Shared control of semi-autonomous vehicles including collision avoidance in multi-agent scenarios

ABSTRACT

A method for providing shared control over movement of a vehicle within a space. The method includes receiving user input related to a velocity and a direction for the vehicle within the space. The method includes processing the user input to selectively adjust the velocity and the direction desired by the user based on a set of predefined constraints to generate a trajectory for the vehicle for an upcoming time period. The method includes operating drive mechanisms in the vehicle based on the trajectory to move the vehicle from a first position to a second position within the space during the upcoming time period. A grid map defining locations of obstacles in the space may be used to define the trajectory to avoid collisions, and a guidance trajectory may be used to further control movement to achieve a desired throughput and control vehicle movement within particular portions of the space.

CROSS-REFERENCE TO RELATED APPLICATIONS

This application claims the benefit of U.S. Provisional Application No.61/878,308, filed Sep. 16, 2013, which is incorporated herein byreference in its entirety.

BACKGROUND

1. Field of the Description

The present description relates, in general, to techniques for remotelycontrolling movements and operations of vehicles such as ground-basedvehicles (e.g., a two-to-four or more wheeled vehicle) and aerialvehicles (e.g., a multi-copter, a drone, or the like), and, moreparticularly, to methods and systems adapted for providing sharedcontrol (i.e., both remote and local operator control) over asemi-autonomous vehicle.

2. Relevant Background

There are many settings where it is desirable to provide shared controlover the operations of a robot such as a terrestrial vehicle or anaerial vehicle. For example, an amusement park may provide a ride inwhich a passenger vehicle is driven along a race track or through anobstacle course, and it may be desirable to allow the passenger to“drive” the vehicle by providing user input indicating a direction theywish to travel and a velocity or speed they want their vehicle to movein the selected direction.

However, the vehicle control is preferably shared to provide collisionavoidance with static obstacles and also with other vehicles (which maybe driven by other ride participants). In other cases, a human operatormay remotely control a flying drone such as a multi-copter in aparticular air space, but the control is shared with a centralcontroller to avoid collisions or achieve other goals.

Human-robot shared control has been applied in a variety of settings,but there remains a need for improved shared-control methods thatenhance human control such as by providing the human operator more localcontrol or operational freedom while still safely avoiding collisions.For example, shared control has been applied in the field of tele-robotoperation for tasks including space exploration and surgery. In the caseof mobile robots, methods have been developed for human interaction witha forklift, and shared control has been used with a formation of aerialvehicles via a haptic device. Work has also progressed for human-robotshared control with semi-autonomous vehicle such as wheelchairs orsimilar vehicles and with self-driving cars and other urbantransporters.

SUMMARY

The present description provides a shared control method (and relativesystem for implementing this control method) for a vehicle (e.g., anywheeled or terrestrial vehicle, any aerial vehicle, and the like). Ahuman driver, who may be in the vehicle or remote to the vehicle,commands (i.e., provides user input to provide) a preferred velocity.The method involves transforming this user input into a collision-freelocal motion that respects actuator constraints (e.g., vehiclekinematics) and allows for smooth and safe control. Collision-free localmotions are achieved with an extension of velocity obstacles that takesinto account dynamic constraints and also a grid-based maprepresentation of the space through which the vehicle is traveling undershare controlled.

To limit the freedom of control of the driver or human operator, aglobal guidance trajectory (or “ghost vehicle”) can be used in theshared control method. The global guidance trajectory may specify theareas (or volumes) where the vehicle is allowed to drive or move in eachtime instance. Further, to improve operations in many environments,schedules are imposed along with or as part of the global guidancetrajectory to ensure the vehicles, even under human control, achievebroadly stated goals of reaching particular places (move from a first orstart position or location to a second or finish position or location)under predefined time schedules.

The low computational complexity of the method makes it well suited formulti-agent settings and high update rates, and a centralized functionand a distributed function (or centralized and distributed algorithms)are described herein that allow for real-time, concurrent control ofmany vehicles (e.g., several to 10 to 40 or more vehicles). Extensiveexperiments have been performed, and the results of these experiments(using a motorized, robotic wheelchair as the vehicle) at relativelyhigh speeds in tight dynamic scenarios are presented in the followingdescription.

More particularly, a method is described for achieving shared controlover movement of a vehicle within a space (2D or 3D space depending onwhether the vehicle is a wheeled/tracked vehicle or boat or is an aerialvehicle). The shared control method includes receiving user inputrelated to a velocity (e.g., speed and direction) for the vehicle withinthe space. The method then includes processing the user input with alocal motion planner selectively adjusting the velocity and thedirection based on a set of predefined constraints (and the presences ofneighboring or other vehicles within the space) to generate a trajectory(which may also be considered a velocity as it defines speed anddirection) for the vehicle in the space. The method further includescommunicating motion control signals including the trajectory to thevehicle and then operating drive mechanisms in the vehicle based on themotion control signals to move the vehicle from a first position to asecond position within the space during an upcoming time period.

In some cases, the trajectory defines a preferred speed for the vehicleand a direction for movement in the space. The set of predefinedconstraints may include a grid map (e.g., image) of the space thatdefines locations of obstacles in the space (physical objects such aswalls and also, in some cases, virtual objects that may be projectedinto the space or be displayed on a monitor/display device in thevehicle). There are at least two ways to treat virtual objects: (1) ifthe virtual object is fixed, it can be placed in the grid map andtreated like a fixed physical object; and (2) if the virtual object itcreated for a transient period only or if it is moving, it can betreated like another vehicle. The processing of the user input includessearching the grid map (e.g., with the vehicle's current position in thespace) to identify the obstacles and to define a path from the firstposition to the second position in the space that avoids the obstacles.

In some implementations of the method, the set of predefined constraintsincludes a guidance trajectory defining a passageway through the spacethat can be traveled by the vehicle without collision with theobstacles, and the trajectory is defined so as to control movement ofthe vehicle to retain the vehicle within the passageway. The passagewaymay include a set of interconnected waypoints in the space and an activearea in which the vehicle may move is associated with each of thewaypoints. At least some of the active areas differ in size or shapedefining differing areas in which the vehicle may move within the space.In some embodiments, the set of predefined constraints includeskinematic data for the vehicle, and the processing based on neighboringvehicles in the space includes processing data for collision avoidancefor other vehicles moving in the space, velocity predictions for ones ofthe other vehicles neighboring the vehicle in the space, and/orpredictions of movements of the other vehicles during an upcoming timeperiod.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG. 1 is a functional block diagram of a shared control system forsemi-autonomous vehicles as described herein;

FIG. 2 illustrates a graph of a wheelchair used as the semi-autonomousvehicle in several experiments implementing a shared control system suchas that shown in FIG. 1;

FIG. 3 illustrates a flow diagram of a method of generating a guidancetrajectories in a grid map of a drive space for vehicles;

FIG. 4 illustrates a roadmap or grid map with guidance trajectoriesdefined by a combination of interconnected waypoints and theirassociated active areas;

FIG. 5 illustrates a graph showing use of the shared control method tocontrol movement of a semi-autonomous vehicle within a drive spacerelative to an obstacle to avoid collision;

FIG. 6 illustrates a graph showing implementation of an avoidanceconstraint to avoid collision between two neighboring vehicles;

FIG. 7 illustrates a graph showing implementation of a dynamicrestriction constraint as part of local motion planning for a vehicleunder shared control;

FIGS. 8A-8F illustrate graphs of two representative examples ofdistributed collision avoidance;

FIGS. 9A-9D illustrate histographs generated based on the shared controlexperimental results;

FIGS. 10A and 10B illustrate histograms of computational times for acollision avoidance algorithm for the distributed and centralized cases;

FIGS. 11A-11C are graphs of vehicles followings two guidancetrajectories;

FIGS. 12A and 12B show histograms of distance from a vehicle to aguidance trajectory;

FIGS. 13A and 13B illustrate graphically schemas of local motionplanning showing candidate local trajectories in relation to candidatereference trajectories and showing an exemplary local trajectory;

FIG. 14 provides a graphic representation of two examples of repulsivevelocities that may be used in a formulation of local motion planning;

FIG. 15 provides a graphical representation of an example of a collisionavoidance constraint for a static rectangular obstacle showing referencevelocities leading to a collision within a given time horizon (note, itscomplement is non-convex and is linearized);

FIGS. 16A-16C provide graphical representations of an inter-agentcollision avoidance constraint illustrating a velocity obstacle inrelative velocity, illustrating projection onto the horizontal plane,and illustrating projection onto a vertical plane, respectively;

FIG. 17 is a functional block diagram or schema of a controller for usein providing shared control over an aerial vehicle such as a quadrotorhelicopter with exemplary variables and frequencies provided;

FIG. 18 is a graph of a simulation of position exchange for tenquadrotor helicopters using the control methods taught herein;

FIG. 19 is a set of graphs showing results of an experiment a singlequadrotor;

FIG. 20 is a set of graphs showing results of an exchange of twoquadrotors in an experiment performed by the inventors;

FIG. 21 is a set of graphs showing results of control experiments usingfour quadrotor helicopters as the aerial vehicles and using twodiffering linearization strategies;

FIG. 22 illustrates a set of graphs showing results of experiments usingtwo quadrotors tracking intersecting trajectories while avoidingcollision locally;

FIG. 23 provides a pair of graphs showing results of experiments usingtwo quadrotors along with a moving human acting as a dynamic obstaclemoving through the test fly space;

FIG. 24 provides graphs showing optimal reference velocity for anexperiment, see FIG. 21, using four quadrotors;

FIG. 25 illustrates a histogram of distance between all agentsaccumulated over a plurality of run experiments;

FIG. 26 illustrates distributed and centralized approaches to localmotion planning to provide cooperative collision avoidance amongdecision-making agents;

FIG. 27 illustrates a functional block drawing or data flow diagramshowing an example of computation of collision constraints according tothe present description; and

FIG. 28 illustrates a functional block or data flow diagram showing anexample of a distributed non-convex search within convex region.

DETAILED DESCRIPTION

In brief, methods and systems are described herein that are designed toprovide shared control of semi-autonomous vehicles (terrestrial orground based vehicles and/or aerial vehicles) based, in many cases, onvelocity space optimization. The method for shared control of asemi-autonomous vehicle is performed based on or using: (1) real-timeinput from a driver or operator of the vehicle; (2) a guidancetrajectory that specifies the areas (which may be 3D spaces or volumesfor aerial vehicles) where the vehicle is allowed to drive or move ineach time instance; (3) avoidance constraints with respect to otheragents (which may be other semi-autonomous vehicles) and/or with respectto a grid map defining the location, size, and shape of static obstacles(real or virtual such as in the form of projected images in thevehicle's drive path/space, images displayed in a vehicle's head's updisplay, and the like); and (4) motion continuity constraints. The localplanner may provide local planning of vehicle movements using efficientcentralized and/or distributed algorithms based on convex optimizationand non-convex search to achieve real-time local planning where an exactgrid-based representation of the map can be used in conjunction with avelocity obstacles framework. This may be desirable to allow forreal-time control of a larger number of vehicles (e.g., 10 to 40 or morevehicles in a space).

FIG. 1 illustrates a shared control system 100 for use in controllingmovement (velocity, direction, and the like) of semi-autonomous vehicles110 within a physical drive space 105. For example, the physical drivespace 105 may be areas of an amusement park ride where it is acceptablefor an operator of one of the vehicles 110 to drive or the space 105 maybe a volume of air space through which the vehicles 110 may fly whencontrolled (e.g., remotely in the case of a drone or multi-copter). Thedriver/operator (not shown) may be seated in or supported on the vehicle110 or may be at a remote location (e.g., user input 114 may be a remotedevice communicating velocity inputs via wireless communications).

The vehicle 110 may include a processor 112 running a local controller116 that controls operations of a drive mechanism(s)/actuator 118 tocause the vehicle 110 to move in a particular direction and velocitywithin the space 105. This control may be based on motion controlsignals 138 from a central control system 130 and also based on inputfrom user input devices 114. For example, a driver/operator 114 may movea steering wheel and accelerator pad to cause the local controller 116to operate the drive mechanism/actuator 118 within an active areadefined/allowed by the motion control signal 138 from central controlsystem to avoid collision and to follow a schedule as the vehicle 110moves along a guidance trajectory 144. The user input device 114 mayalso take the form of a joystick that can be manipulated by thedriver/operator of vehicle 110.

The local controller 116 and/or other components on the vehicle 110 mayoperate to transmit (wirelessly in most cases) user input data 119 inresponse to operations of the user input device(s) 114 by thedriver/operator, and the user input data 119 typically will include auser-preferred velocity (e.g., direction and speed for the vehicle 110in drive space 105). The communicate data 119 may also include sensor ortracking system data that is useful for determining the present locationof other/neighbor vehicles 110 in the space and, in some cases, theirpresent velocity, both of which can be used by the control system 120 incontrolling the vehicle 110 via motion control signals 138 to avoidcollisions among the vehicles 110 in the space 105.

The system 100 further includes a central control system 120 that maytake the form of a computer or computer-based system speciallyconfigured to control the vehicles 110 via motion control signals 138.Particularly, the control system 120 includes one or more processors 122managing operations of input/output (I/O) devices such as a monitor, akeyboard, a mouse, a touchpad, a touchscreen, and the like for receivinginput from an operator and for displaying data (such as displayingpresent locations of vehicles 110 in space 105 and the like). Theprocessor 122 also runs or executes code in a computer readable mediumto provide a controller module 130, which provides all or a largeportion of the shared motion control described herein. The controllermodule 130 may include a local motion planner 132 to generate the motioncontrol signals 138, and, to this end, the local motion planner 132 mayinclude a trajectory controller 134 along with an optimization engine136 whose functions and design are described in detail below.

The processor 122 manages one or data storages devices or memory 140 tostore (at least temporarily) data used by the controller module 130 toprovide the shared control of the vehicles 110. As shown, for example,the memory 140 may be used to store received user input and sensor data1442 from the vehicle 110. Further, the memory 140 may store a guidancetrajectory 144 defined by the controller module 130 for controlling thevehicle 110 including defining active areas 145 within the drive space105 in which the vehicle 110 may be driven by an operator/driver whilefollowing a ride path or other planned trajectory. The memory 140 alsostores a number of optimization constraints 150 that are used by theoptimization engine 136. The constraints 150 may include a grid map 152that maps physical obstacles 154 and, in some embodiments, virtualobjects 156 (e.g., displayed 2D or 3D images of objects projected intospace 105 or displayed to within or nearby the vehicle 110 to appear tobe in the space 105) to locations or positions within the drive space105. The controller module 130 may search the grid map 152 based on apresent location of the vehicle 110 to generate a motion control signal138 that avoids collision with obstacles.

Additionally, the memory 140 may be used to store vehicle propertiesand/or kinematics 158 for the vehicle 110 that can be used to determinethe motion control signal 138 (e.g., an acceptable local trajectory fora next control cycle or at a present time). Other constraints 150 mayinclude data on other vehicles/agents 160 that may be used to avoidcollisions and may be determined from sensor data 119 from the vehicle110 or from an external tracking system (not shown) operating todetermine present locations of each of the vehicles 110 in the drivespace 105. The optimization constraints 150 may include velocitypredictions 164 for other or neighbor vehicles 110 that may be nearby tothe vehicle 110 (“ego vehicle”) in the drive space 105. This informationcan be used by the optimization engine 136 to control the vehicle 110via signals 138 so as to avoid collisions with other flying or airbornevehicles 110 in the 3D space 105.

With the system 100 understood and in mind, it may be useful to providean overview of the shared control method that may be provided byoperation of the system 100 and similar systems. Then, the descriptioncan proceed with explanation of a useful vehicle model and control, anexemplary guidance trajectory, a shared control technique, local motionplanning, algorithms for implementing the methods taught herein, andexperimental results.

Consider a group of n vehicles of which m are controlled, either in adistributed or centralized way. The position, velocity, and accelerationof a robot, i, are denoted by p_(i) ∈

², v_(i)={dot over (p)}_(i), and a_(i)={umlaut over (p)}_(i)respectively. A disk, D(p,r), of radius r_(i) centered at p_(i) definesits enveloping shape. The enlarged obstacle map is given by a staticgrid map O_(r) of the positions p where a disk D(p,r) is in collisionwith a static obstacle.

The driver/operator of the vehicle (or robot) commands the vehiclethrough the vehicle's user input devices by specifying a desiredvelocity. A guidance trajectory along with or together with active areas(i.e., areas (or spaces) where the vehicle is allowed to drive (or fly))can be specified to: (1) guide the motion of the vehicle; (2) limit itsmovements; (3) guarantee that the vehicle has a certain performance evenin the case where the driver does not specify a velocity; and (4)control the throughput of vehicles throughout a drive space or ridearea. In brief, the desired velocity and the guidance trajectory (twoinputs to the controller module) are combined and a local trajectory iscomputed (which is used to generate motion control signals, e.g., afteroptimization, for the vehicle's drive mechanism/actuator).

For each vehicle, the set of local trajectories is given by a controller(of sufficient continuity, C², in this description) towards astraight-line reference trajectory at velocity u ∈

² (see FIG. 2). This provides a low dimensional parameterization (givenby u) of the local motions and allows for an efficient optimization inreference velocity space to achieve collision-free motions that respectthe dynamic constraints of the vehicle. In each time step of the localplanner, the motion is obtained by computing an optimal referencevelocity, u*, such that its associated trajectory is collision free,satisfies the motion and guidance constraints, and minimizes thedistance to a preferred velocity, ū. The preferred velocity takes intoaccount the driver input, a guidance trajectory, and the neighboringobstacles.

Throughout this description, x denotes scalars, x vectors, x=∥x∥ itsEuclidean norm, X matrices, X sets, and X⊕y their Minkowski sum. Thesuper index *^(k) indicates the value at time t^(k) and subindex *_(i)indicates agent i and x_(ij)=x_(i)−x_(j) relative vectors. Time relativeto the current time step is employed when appropriate and denoted by{tilde over (t)}=t−t^(k). In practice, estimated states are used, but,to simplify the notation, no distinction is made herein betweenestimated and real states.

Turning now to a vehicle model and control, given a reference trajectoryat constant velocity p_(ref)(t)=p^(k)+u{tilde over (t)} for {tilde over(t)} ∈ [0,∞), with p^(k) being the position of i at the currenttime-step k and u being the reference velocity, one can consider atrajectory tracking controller f(z^(k),u,t)=p(t) that is continuous inthe initial state, z^(k)=[p^(k),{dot over (p)}^(k),{umlaut over(p)}^(k), . . . ] of the agent and that is converging to the referencetrajectory. The set of feasible motions is given by:

(z ^(k))={u ∈

², such that the trajectory f(z ^(k) ,u,t) respects all dynamicrestraints}  Eq. (1)

This set can be pre-computed by forward simulation and depends on {dotover (p)}^(k) and {umlaut over (p)}^(k). A trajectory controllerappropriate for the vehicle kinematics can then be formulated.

With regard to such a trajectory controller, and recalling the methodsherein described are independent of the chosen controller, one suchexample is as follows. If the system is modeled as the second orderintegrator (continuity in acceleration) and the appropriate statefeedback control low towards the reference trajectory is formulated, thetrajectory f(z^(k),u,t) is given (for t<0) by:

p(t)=p ^(k) +u t +(({dot over (p)} ^(k) −u+(w ₁ −w ₀)F){tilde over(t)}−F)e ^(−w) ⁰ ^(t) +Fe ^(−w) ¹ ^(t)   Eq. (2)

with F=({umlaut over (p)}^(k)+2w₀({dot over (p)}^(k)−v_(∞)))/(w₀−w₁)²and w₀ and w₁ being design parameters related to the decay of theinitial velocity and acceleration. Saturation limits are added, e.g.,maximum linear and angular speed (|v₁|≦v_(1,max), |v₂|≦v_(2,max)) andmaximum linear and angular accelerations (|{dot over (v)}₁|≦v_(1,max),|{dot over (v)}₂|≦{dot over (v)}_(2,max)).

Turning now to kinematics of the vehicle, the inventors employedwheelchairs for the semi-autonomous vehicles that can have sharedcontrol. With regard to wheelchair (vehicle, in this example)kinematics, the unicycle model is used, with p the point in-between bothtractor wheels, δ the orientation of the vehicle, and v₁, v₂ the linearand angular speed of the platform. The graph 200 of FIG. 2, which showsthe wheelchair/vehicle 210 with its two rear driving wheels 212 and tofront castor wheels 214 and which shows a reference trajectory given byu and an executed trajectory, and the kinematics can be stated as:

[ {dot over (p)} , {dot over (δ)}]′=[cos δ, sin δ, 0]′v ₁+[0, 0, 1]′v ₂  Eq. (3)

A (fully controllable or holonomic) point p= p+[cos δ,sin δ]^(T)Δ to thefront of the vehicle axles can then be considered, where Δ>0 defines themaneuverability of the vehicle. The following relations hold:

{dot over (p)}=ξ, v ₁=ξ·[cos δ,sin δ]^(T) , v ₂=ξ·[−sin δ,cosδ]^(T)/Δ  Eq. (4)

The local trajectory is applied at this point, and a circle of radius ris considered as the enveloping shape of the vehicle.

With regard to use of a guidance trajectory, it may be desirable forsome applications of shared control to have higher control over themotion of the vehicles. In these cases, a roadmap or set of guidancetrajectories can be created. A method 300 of creating a roadmap or setof guidance trajectories is shown in FIG. 3. The method 300 starts atstep 305 such as with selecting a drive area or space in which a vehicleis allowed to move under driver control/input such as with regard todirection and speed. The method 300 continues at 310 by providing astatic map of the selected drive space that includes locations, shapes,and sizes of obstacles. The roadmap may be computed automatically or becreated via waypoints connected by straight lines as shown in FIG. 3.This representation does not require one to account for the dynamics ofthe vehicle nor to perfectly fit the map since both of these constraintsor parameters are considered by the local motion planner.

At step 320, a set of waypoints is specified, and, for each waypoint, anarrival time, T_(l), (or, alternatively, a speed over the segment) andan active area (where the vehicle is allowed to freely move) of apredefined radius, ρ_(l), around the waypoint is provided. At 340, themethod 300 continues with connecting consecutive waypoints. Then, at350, the method 300 involves generating or defining the guidancetrajectory along with the active area around it (e.g., by interpolationor the like). The method 300 then ends at 390. In practice, when two ormore active areas (e.g., from different trajectories) intersect, thevehicle can be assigned to an active area depending on its currentposition and specified attributes of the guidance trajectories, such asif more than one vehicle can follow that trajectory or if the vehiclemust stay in the current one.

In the method 300, one can select the trajectory, s, that minimizes theweighted distance to the current point on the guidance trajectory,q^(s), relative to the current radius of the active area, ρ^(s):

arg_(s) ^(min)(1−σ^(s))∥p−q^(s)∥/ρ^(s)   Eq. (5)

where σ^(s) ∈ (0,1) is an attribute indicating the predilection to stayin one trajectory. This framework provides an intuitive way to guide ateam of vehicles throughout the drive space while giving the vehiclesand their drivers/operators limited freedom in their movement via theactive areas. An active area with a large radius, ρ, implies greaterfreedom of movement for the vehicle while an active area with a smallradius, ρ, imposes less freedom and accurate tracking of the vehiclealong and within the guidance trajectory.

FIG. 4 illustrates a grid map 400 of a drive space 410 that may begenerated with the method 300 of FIG. 3. The drive space 410 includesseveral rooms and corridors with these available spaces/areas fordriving shown at 426 (with lighter or no shading) and occupied spaces orphysical/virtual objects shown at 420 (with darker shading). In thisexample, nine waypoints, w_(i), are created as shown with exemplarywaypoints 430 with arrival times, T_(i), and a number of differentradii, ρ_(i), for their associated active areas 434. The waypoints 430are connected by straight lines as shown at 438, and, in betweenwaypoints, the ideal trajectory and active area is given byinterpolation. In the grid map 400 with the guidance trajectories andactive areas, waypoints 3 a and 3 b represent a bifurcation and waypoint7 has a large active area where the vehicles are allowed to freelydrive.

Now, with regard to shared control, in each time step, for each vehiclei, a preferred velocity ū_(i) is computed, based on the input from thedriver, the guidance trajectory, and the neighboring obstacles asfollows:

ū _(i)=μ₁(μ₂ u _(i) ^(joy) +u _(i) ^(traj))+(1−μ₁)u _(i) ^(A) *+u _(i)^(rep)   Eq. (6)

where μ₂=1 if the vehicle is inside the active area and μ₂=0 otherwise,and μ₁=1 if q is in line of sight of the vehicle and μ₁=0 otherwise (seeFIG. 5). In practice, a small hysteresis is introduced to μ₁ and μ₂ toavoid oscillations. In normal operations, μ₁=μ₂=1. Each velocity term iscomputed as follows. The input velocity u_(i) ^(joy) is proportional tothe joystick position (in relative frame of the vehicle). The guidancevelocity u_(i) ^(traj) (optional term) is given by a proportionalcontroller to the guidance trajectory as:

u _(i) ^(traj) =K _(p)(q _(i) −p _(i))+{dot over (q)} _(i)   Eq. (7)

where q_(i) is the ideal position on the guidance trajectory at thecurrent time. The repulsive velocity u_(i) ^(rep) is computed with apotential field approach that pushes the vehicle away from both staticand dynamic obstacles. A linear function weighting the distance to othervehicles is used as follows:

$\begin{matrix}{u_{i}^{rep} = {{{Vr}\frac{\overset{\_}{D} - {d\left( {p_{i}} \right)}}{\overset{\_}{D}}n_{i,}} + {\sum\limits_{j{p_{ij} < D}}{{Vr}\frac{D - p_{ij}}{D - r_{i} - r_{j}}\frac{p_{ij}}{p_{ij}}}}}} & {{Eq}.\mspace{14mu} (8)}\end{matrix}$

where V_(r) is the maximum repulsive velocity, D and D are the maximumdistance from which a repulsive force is added, d(p_(i),O) is thedistance to the closest static obstacle within the O_(rj) map, andn_(i,O), is the normal vector from it. In order to avoid corner issues,not only the closest point is considered but also an average over theclosest points. The norm of u_(i) ^(rep) is then limited to ∥u_(i)^(rep)∥≦V_(r).

In cases where the guidance point, q_(i), is not in direct line of viewfrom the current position, p_(i), a preferred velocity solely based onthe previous terms could be prone to deadlocks. To avoid this, a term,u_(i) ^(A)*, computed from a global path from p_(i) to q_(i) taking intoaccount the map O_(r) but ignoring the kinematic constraints and othervehicles may be added. This term is given by the first control input ofa standard A* search in the grid map.

FIG. 5 illustrates a graph 500 representing implementation of suchshared control. Components of the preferred reference velocity for avehicle 510 within the active area 520 of the current point, q, on theideal trajectory within the drive space. An obstacle 530 is shown in thegraph with the shared control functioning to avoid collision with theobstacle.

With regard to local motion planning, in each control loop, given thepreferred velocity, ū_(i), of the ego vehicle and the current positionand velocity of neighboring vehicles, a local trajectory is given by anoptimal reference velocity, u*_(i), is computed by solving anoptimization problem in reference velocity space. The optimizationproblem may be formulated as a combination of a convex optimization withquadratic cost and linear and quadratic constraints and a non-convexoptimization.

Two alternative formulations are presented: (1) a centralizedoptimization where the optimal reference velocities of all vehicles,u*_(1:m)=[u*₁, . . . , u*_(m)], are jointly computed and (2) adistributed optimization where each vehicle, i, independently solves anoptimization where its optimal reference velocity, u*_(i), is computed.The position p_(j) and velocity v_(j) of neighboring agents is known.The optimization cost is given in two parts. The first one is aregularizing term penalizing changes in velocity, and the second oneminimizing the deviation to the preferred reference velocity. For thecentralized cast the cost may be provided as:

C(u _(1:m)):=K _(o) ∥u _(1:m) −v _(1:m)∥² +∥u _(1:m) −ū _(1:m)∥²   Eq.(9)

where K_(o) is a design constant. For the distributed case, the cost maybe provided as:

C(u _(i)):=K _(o) ∥u _(i) −v _(i)∥² +∥u _(i) −ū _(i)∥²   Eq. (10)

The kinematic constraints of the vehicle are included where thevehicle's radius is enlarged by a value ε>0 and the local trajectoriesare limited to those with a tracking error below ε with respect to thereference trajectory (parameterized by u).

With regard to a first constraint (“Constraint 1”) concerning avoidanceof other agents, for every pair of neighboring agents i≦m and j≦n, theconstraint is given by the reference velocities such that∥p_(i)−p_(j)+(u_(i)−u_(j))t∥≧r_(i+j)=r_(i)+ε_(i)+r_(j)+ε_(j), for all t∈ [0,τ]. This constraint can be rewritten as u_(i)−u_(j) ∉ VO_(ij)^(τ)=∪_(t=0) ^(τ)((D(p_(j),r_(j)+ε_(j)) ⊕ D(pi,ri+εi/t) representing atruncated cone. For example, FIG. 6 illustrates with graph 600 theimplementation of the constraint of avoidance of other vehicles oragents. This constraint is shown as area 630 for avoidance between twovehicles 610 and 620, and vehicle avoidance constraint is approximatedby three linear constraints as shown in the graph 600. This constraintcan then be computed if the distance between the two agents is below athreshold (p_(ij)<K_(d)). The non-convex constraint

²\VO_(ij) ^(τ) is approximated by three linear constraints of the formn_(ij) ^(l)·u_(ij)−b_(ij) ^(l) as:

$\begin{matrix}{{{\begin{bmatrix}{\cos \left( \gamma^{+} \right)} \\{\sin \left( \gamma^{+} \right)}\end{bmatrix}u_{ij}} \leq 0},{{{- \frac{p_{ij}}{pij}} \cdot u_{ij}} \leq \frac{p_{ij} - r_{i + j}}{\tau}},{{\begin{bmatrix}{\cos \left( \gamma^{-} \right)} \\{\sin \left( \gamma^{-} \right)}\end{bmatrix}u_{ij}} \leq 0}} & {{Eq}.\mspace{14mu} (11)}\end{matrix}$

where γ⁺=α+β, γ⁻=α−β, α=a tan 2(−p_(ij)) and β=a cos (r_(i+j)/p_(ij)).The first and last constraints represent avoidance to the right and tothe left, and the middle one a head-on maneuver (collision-free up tot=τ). The linearization of

²\VO_(ij) ^(τ) is obtained by selecting the linear constraint withmaximum constraint satisfaction for the current relative velocity(min_(l)(n_(ij) ^(l)v_(ij)−b_(ij) ^(l))). Other sensible choices includelinearization with respect to the preferred velocity ū_(ij) or fixedavoidance side (right/left). For j≦m, this linear constraint is added tothe centralized optimization. For j>m (dynamic obstacles) or whendistributed, it can be rewritten as follows.

In the distributed case, all agents are considered as independentdecision-making agents solving their independent optimizations. Toglobally maintain the constraint satisfaction and avoid collisions, anassumption on agent j's reference velocity should be determined.Variable sharing of avoidance effort might be considered withΔv_(i)=λΔv_(ij) and the assumption that Δv_(j)=−(1−λ)Δv_(ij). Forcollaborative agents that equally share the avoidance effort, λ=0.5. Ifit is considered that agent j ignores agent i and continues with itscurrent velocity, then λ=1 and full avoidance is performed by agent i.For λ ∈ [0,1], the constraint is given by:

$\begin{matrix}{{n_{ij}^{l} \cdot u_{ij}} = {{\frac{n_{ij}^{l}}{\lambda} \cdot \left( {u_{i} - {\left( {1 - \lambda} \right)v_{i}} - {\lambda \; v_{i}}} \right)} \leq b_{ij}^{l}}} & {{Eq}.\mspace{14mu} (12)}\end{matrix}$

With regard to a second constraint (“Constraint 2”) concerning activeareas, this constraint guarantees that the agent locally remains withinthe active area. For agent i≦m, the constraint is given by∥p_(i)−q_(i)+(u_(i)+{dot over (q)}_(i))τ∥²≦p_(i) ² with q_(i) being thepoint on the guidance trajectory for the current time, {dot over(q)}_(i) being speed, and ρ_(i) the radius of its associated active area(by interpolation between the previous and next waypoint). With theconstant velocity assumption for the guidance trajectory, if the vehicleis initially within the active area, this constraint guarantees that thevehicle remains inside for time t≦τ. If the vehicle is initiallyoutside, the constraint guarantees that the vehicle enters within timeτ. The constraint can be written as:

$\begin{matrix}{{{u_{i}^{T}u_{i}} + {2\left( {\frac{p_{i} - q_{i}}{\tau} - {\overset{.}{q}}_{i}} \right)^{T}u_{i}} + {\left( {\frac{p_{i} - q_{i}}{\tau} - {\overset{.}{q}}_{i}} \right)}^{2}} \leq \frac{\rho_{i}^{2}}{\tau^{2}}} & {{Eq}.\mspace{14mu} (13)}\end{matrix}$

This is a quadratic constant (circle in reference velocity space) thatis ignored if no active areas are given.

With regard to a third constraint (“Constraint 3”) concerning avoidanceof static obstacles, for agent i≦m, the constraint is given by thereference velocities such that the new positions are not in collisionwith the enlarged obstacle map, p_(i)+u_(i)t ∉ O_(r) _(i) _(+ε) _(i) ,for all t ∈ [0,τ]. This constraint is kept in its grid-based form(obstacles in FIG. 3) to allow the vehicle full navigation capabilityincluding being able to move close to the static obstacles and to goinside and/or through small openings. Given a known map, O₀, theenlarged map, O_(r) _(i) _(+ε) _(i) , can be precomputed for severalvalues of ε_(i).

With regard to a fourth constraint (“Constraint 4”) concerning dynamicrestrictions, each agent should remain within ε_(i) of its referencetrajectory. For a maximum tracking error, ε_(i), and a current state,z_(i)=[p_(i),{dot over (p)}_(i),{umlaut over (p)}_(i)], the set ofreference velocities, u_(i), that can be achieved with position errorbelow ε_(i) is given by R_(i):=R(z_(i),ε_(i)) such that:

R _(i) :={u _(i) ∈

(z _(i))|∥(p _(i) +tu _(i))−f(z _(i) ,u _(i) ,t)∥≦ε_(i) , ∀t>0}  Eq.(14)

If the trajectory controller defined in part by Eq. (2) is used, the setof reachable reference velocities, R_(i), depends on {dot over (p)}_(i),{umlaut over (p)}_(i), and ε_(i). A mapping, γ, from initial state, {dotover (p)}, {umlaut over (p)}, and the reference velocity, u, to amaximum tracking error is precomputed and stored in a look up table as:

$\begin{matrix}{{\gamma \left( {u,\overset{.}{p},\overset{¨}{p}} \right)} = {\begin{matrix}\max \\{t > 0}\end{matrix}{{\left( {p + {tu}} \right) - {f\left( {z,u,t} \right)}}}}} & {{Eq}.\mspace{14mu} (15)}\end{matrix}$

An example of this constraint is shown in FIG. 7. FIG. 7 illustrateswith graph or plot 700 the implementation of the dynamic restrictionsconstrain with a trajectory controller. As shown, a vehicle may have acurrent velocity as shown at 710, and the areas 720 and 730 representreachable reference velocities, R_(i), for the representative currentvelocity 710. The plot/graph 700 is in reference velocity space alignedwith the vehicle axis (e.g., tangential/normal), with forward velocitiesbeing displayed. A bounding box is given by four linear constraints ofthe form H_(l)={u_(i)|A_(l)u_(i)≦b_(l)} is computed such thatR(z_(i),ε_(i)) ⊂ ∪_(i=1) ⁴H_(l). These linear constraints are includedin both the centralized and decentralized optimizations to reduce thesearch space.

With regards to a useful optimization algorithm and guarantees, theoptimization includes two types of constraints: (1) convex (linear andquadratic) and non-convex (grid-based). To efficiently find a solution,the optimization is divided into two parts. First, a convex subproblemis solved resulting in u_(i) ^(c), and, second, a search within thegrid-based constraints is performed that is restricted to the convexarea defined by the linear and quadratic constraints. The set of convexconstraints (Constraints 1 and 2 and bounding box of Constraint 4) isdenoted by C. The set of non-convex constraints (Constraints 3 and 4 foragent I, with respect to a grid of the same resolution) is denoted by{tilde over (C)}_(i). For both the centralized (N=m) and the distributed(N=1, without loss of generality) optimizations, the algorithm may bestated as:

Input distributed: z₁, ū₁ and r₁ + ε₁; p_(j), {dot over (p)}_(j) andr_(j) ∀j neighbor of 1. Consider ε_(j) = ε₁. Input centralized: z_(i),ū_(i) and r_(i) + ε_(i)∀i ≦ m; p_(j), {dot over (p)}_(j) and r_(j) ∀j >m neighbor of i ≦ m. Result: u_(1:N)* = [u₁*, ..., u_(N)*] Computeconstraints (See Eq. (5)) u_(1:N) ^(c) ← solution 2N-dimensional convexoptimization with quadratic cost (Eqs. (9)-(10)) and convex constraintsC. //Wave expansion from u_(1:N) ^(c within convex area C.) Initializeshorted list L (increasing cost) with u_(1:N) ^(c) while L = Ø do  u_(1:N) ← first point in L; L := L\u_(1:N); feasible := ‘true’;   fori = 1 to N do     if feasibleDynamics(u_(i))=’false’ orfeasibleMap(u_(i))=’false’     then       L ← expandNeighbors(L,u_(1:N), i, 1);       feasible := ‘false’; break;     end if   end for  if feasible = ‘true’ then return u_(1:N)* = u_(1:N) end if end while;return 0.

Function feasibleDynamics(u_(i)) checks in a precomputed grid if thetracking error is below ε_(i) given the initial state of the vehicle:

if u_(i) ∈ R_(i) (See Eq. 14) then return ‘true’; else ‘false’  Eq. (21)

Function feasibleMap(ui) checks if ui leads to a trajectory in collisionwith static obstacles given by the grid map O. This is efficientlychecked in the precomputed dilated map, O_(r) _(i) _(+ε) _(i) , (seeConstraint 3):

if segment (p _(i) ,p _(i) +u _(i)τ) ∩ O _(r) _(i) _(+ε) _(i) =Ø thenreturn ‘true’  Eq. (22)

The function expandNeighbors adds the neighboring grid points if theyare within the convex region defined by the convex constraints in C, andthey were not previously explored. This algorithm is recursive for N>1,although only one level of recursion is used to reduce computationaltime at the expense of optimality:

Function L ← expandNeighbors(L, u_(1:N) ^(in), k, rec); for each8-connected grid neighbor u_(k) of u_(k) ^(in) do  Consider u_(1:N) =[u₁ ^(in), ..., u_(k−1) ^(in), u_(k), u_(k+1) ^(in), ..., u_(N) ^(in)]. if {u_(1:N) not previously add to L} and  {u_(k) not checked asunfeasible w.r.t. constr. {tilde over (C)}_(i)} then    if u_(1:N)satisfies convex constraints C      add u_(1:N) to L    else      if{rec=1} and {∃ constr. type 1 unfeasible} then        Select constr.which maximizes n · u_(1:N) − b > 0.        j := “index of second agentin the selected constraint”        if j ≦ N then L ← expandNeighbors(L,u_(1:N), j, 0) end if; end if; end if; end for

If the optimization is unfeasible, constraints 2 are removed and theoptimization is recomputed. If still unfeasible, agents i≦N decelerateon their last feasible local trajectory following the time remap:

γ(t)=−t _(f) ²/(2τ)+(1+t _(f)/τ)t−t ²/(2τ)   Eq. (16)

where t_(f) is the time where the last feasible local motion wasobtained. This reparameterization of the trajectories guarantees thatthe vehicles stop within the time horizon of the local planner, unlessthe optimization becomes feasible in a later time step.

With regard to computational complexity, with a limit in the number oflinear constraints for collision avoidance, the complexity of thealgorithm can be kept linear with the number of agents, although thecentralized is of higher cost. If distributed, for each agent, theoptimization is made up of two variables, one quadratic constraint, fourlinear constraints (bounding box of Constraint 5), a maximum of m linearconstraints of Type 1 (in practice limited to a constant K_(c)) and awave expansion within the 2D grid. If centralized, the optimization ismade up of 2n variables, n quadratic constraints, less than

${4n} + \frac{n\left( {n - 1} \right)}{2} + {n\left( {m - n} \right)}$

linear constraints (limited to 4n+nk_(c)) and a joint wave expansion inn 2D grid maps.

With regard to safety guarantees, if feasible, the local trajectoriesare collision free up to time τ, with the assumption that other vehiclesfollow the same algorithm or maintain constant velocity. If unfeasible,no collision-free solution exists that respects all the constraints. Ifthe time horizon is longer than the required time to stop, safety ispreserved if all vehicles drive their last feasible trajectory with atime reparameterization to reach stop before a collision arises:

{dot over (γ)}(t _(f)+τ)=(1+t _(f)/τ)−(t _(f)+τ)/τ=0   Eq. (17)

Note, though, that the optimization can be unfeasible due to severalcauses. First, there may not be enough time to find the solution withinthe allocated time. Second, there may be differences between the modelvehicle and the real world vehicle. Third, there may be noise inlocalization and estimation of the vehicle's state. Fourth, optimizationmay be unfeasible due to the limited local planning horizon togetherwith over simplification of motion capabilities by reducing them to theset of local motions. Fifth, if distributed, unfeasibility may be causedgiven the use of pair-wise partitions of velocity space with either theassumption of equal effort in the avoidance or constant speed. Hence,not all world constraints are taken into account for the neighboringagents/vehicles. As a result, a vehicle may have conflicting partitionswith respect to different neighbors, static obstacles, and/or kinematicsthat render optimization unfeasible for this vehicle.

With regards to deadlock-free guarantees, for a single agent/vehicle,deadlock-free navigation is achieved in part due to the u_(A)* term inthe optimization that drives the vehicle toward its goal position orguidance trajectory. In the multi-agent (or vehicle) scenario, deadlocksare still possible although in degenerated situations. Giving priorityto those agents/vehicles further in their guidance trajectory can helpresolve this situation, though. The input from the driver of the vehiclecan also act as a deadlock-breaking input.

Turning to experimental results, the algorithms have been extensivelytested in simulation with larger groups of robots/agents. Due to spacelimitations in the test setup, the analysis relied upon real wheelchairsfor the vehicles with a driver (see FIG. 2). Each vehicle had anenclosing radius of 1 m, limits v_(1,max)=3 m/s, v_(2,max)=2 rad/s, {dotover (v)}_(1,max)=2 m/s², {dot over (v)}_(2,max)=20 rad/s², {dot over(v)}_(1,min)=−1.1 m/s², and a maximum sideways acceleration of 1.5 m/s².Vehicles were tracked by an overhead tracking system and controlled froma central computer at 30 Hz. Velocity commands from the driver weretransmitted to the central computer at the same rate. Computations wereperformed using an i5 3 GHZ PC.

As part of the experiment with shared control, semi-autonomous drivingwas tested without a guidance trajectory and where each vehicle wasdriven by a human that inputted a desired velocity, u^(joy). Over 50hours of experiments showed that the algorithms described above are safeand collisions with walls and other obstacles as well as with othervehicles were avoided. FIGS. 8A-8F illustrate graphs 810, 820, 830, 840,850, and 860 that show, respectively, examples of robot (or vehicle) andobstacle (or wall) collision avoidance, robot-to-robot (orvehicle-to-vehicle collision avoidance, distance velocity, relativevelocity (negative when towards each other and with time-steps where theoptimization was unfeasible are marked in gray in the background andwhere a zero distance indicates that two objects touch), and vehicleposition (with the input velocity from the driver being displayed withdashed arrows and the safe vehicle command with solid arrows).

FIGS. 8A-8F show two representative examples of the distributedcollision avoidance, where the driver-commanded velocity, u^(joy), isshown with arrows in the graphs 850 and 860. In graph 850, safe andsmooth velocity between 1 and 2.5 m/s was achieved in very closeproximity to a wall when the driver was commanding the vehicle towardsthe wall. The vehicle slightly slows down as it gets closer to the wall(obstacle). In the graph 860, a frontal collision with another vehicleis avoided where a relative velocity of about 5 m/s in very closeproximity (e.g., below 6 m) with a neighboring vehicle was handled. Inthis example, the optimization became unfeasible during 0.15 seconds,which was mostly due to lack of reactiveness in the low level controllerand unmodeled dynamics that slowed the vehicle. This renders thealgorithm feasible in subsequent time steps.

FIGS. 9A-9D show four histograms 910, 920, 930, and 940 over the sharedcontrol experiments Linear and angular velocities are shown withhistograms 910 and 920 while histograms 930 and 940 show input velocityfrom the driver relative to the vehicle. From these histograms, it canbe seen that even if the velocity input by the driver seems to follow abang-bang control of zero (e.g., maximum velocity as seen in histogram930 of FIG. 9C), the algorithm adapts the velocity of the vehicle in therange of −1 to 3 m/s to remain safe (see, for example, FIG. 9A andhistogram 910). The driver stops at some times as can be seen from theexperiment results. The angular velocity is smooth and within the limits(see, for example, FIG. 9B and histogram 920), while the orientation ofthe desired velocity, (see FIG. 9B in vehicle reference frame) is mostlycentered in the forward direction and presents clear peaks at 0, ±90,and 180 degrees.

FIGS. 10A and 10B shows histograms 1010 and 1020 (distributed andcentralized, respectively) in log scale of the computational time of thecollision avoidance algorithm. If no solution is found in 35 ms in thisexperiment, the algorithm was configured to return unfeasible for thattime step. FIGS. 10A and 10B show, in log scale to emphasize theinfrequent worst cases, statistics of the computational time for thecollision avoidance optimization. For the distributed algorithm as shownin histogram 1010 (per vehicle), a solution is usually found in lessthan about 1.5 ms. Higher times, e.g., below 6 ms in all experiments,depend on the non-convex search within the convex region. Similarcomputational times per vehicle were observed for experiments withlarger numbers of vehicles in simulation. For the centralized algorithmas shown in histogram 1020 (two vehicles), higher computational timeswere observed for the worst case, and the inventors believe this mayhave been due to inefficient handling of high-dimensional sorted listsand other implementation inefficiencies.

With regard to constrained driving experiments using a guidancetrajectory along with a grid map of obstacles, FIGS. 11A-11C show graphs1110, 1120, and 1130 providing vehicle distance and linear velocitiesover time (in FIGS. 11A and 11B, respectively) and vehicle position (inFIG. 11C) with obstacles shown at 1132, areas covered by vehicle at1134, and lines 1136 providing a time indicator. FIGS. 12A and 12B showhistograms 1210 and 1220 (with and without a driver, respectively) ofdistance from vehicle to guidance trajectory relative to active arearadius. FIGS. 12A and 12B show statistics over several experiments ofvehicle distance to the guidance trajectory relative to the active arearadius. For a vehicle without a driver (as seen in histogram 1210), the10 percent offset is due to the proportional tracking controlleremployed. For a vehicle with a driver (as seen in histogram 1220), itcan be observed that the driver is able to freely move within the activearea, but the optimization algorithm successfully constrains the motionof the vehicle so as to avoid collisions with obstacles or othervehicles.

As will be appreciated based on the above description, a method istaught for shared control of a vehicle. In this shared control method,the driver commands the vehicle by specifying a preferred velocity. Thespecified velocity is then transformed into a local motion that respectsthe actuator limits of the vehicle and is collision free with respect toother moving vehicles and static obstacles (physical or virtual) givenby (or defined by) a grid map of the drive space. This allows for smoothand safe control of the vehicle. In order to limit the freedom of thedriver, a global guidance trajectory can be included in some embodimentsof the shared control method, and the global guidance trajectoryspecifies the areas (or spaces) where the vehicle is allowed to drive ineach time instance. Good performance has been observed in extensiveexperimental tests at speeds of up to 3 m/s and in close proximity toother vehicles and obstacles (e.g., walls). Further, the lowcomputational time of the optimization algorithm allows for real-timecontrol of numerous vehicles in the drive space.

The above description teaches the inventors' shared control for vehiclesthat are wheeled or driving on the ground in a 2D drive space. Theinventors further explored application and/or extension of their sharedcontrol method (and associated system) to collision avoidance for aerialvehicles especially in multi-agent (or multi-aerial vehicle) scenarios.The following paragraphs describe an investigation of local motionplanning and collision avoidance for a set of decision-making agents(vehicles) navigating or flying in 3D space. The shared control methodis applicable to agents that are heterogeneous or homogeneous in size,dynamics, and aggressiveness. The method may use and expand upon theconcept of velocity obstacles (VO), which characterizes the set oftrajectories that lead to a collision between interacting agents.

Motion continuity constraints are satisfied by using a trajectorytracking controller and by constraining the set of available localtrajectories in an optimization. Collision-free motion is obtained byselecting a feasible trajectory from the VO's complement, wherereciprocity can also be encoded. The following three algorithms forlocal motion planning are presented herein: (1) a centralized convexoptimization in which a joint quadratic cost function is minimizedsubject to linear and quadratic constraints; (2) a distributed convexoptimization derived from the centralized convex optimization; and (3) acentralized non-convex optimization with binary variables in which theglobal optimum can be found but at a higher computational cost. Thefollowing description also presents results from experiments with up tofour vehicles/agents in the form of physical quadrotors (ormulti-copters) flying in close proximity and with two quadrotorsavoiding a human in the 3D space (or flying space or air space).

The last decade has seen significant interest in unmanned aerialvehicles (UAVs) including multi-rotor helicopters (or multi-copters) dueto their mechanical simplicity and agility. Successful navigationalcontrol builds on the interconnected competences of localization,mapping, and motion planning and/or control. The real-time computationof global collision-free trajectories in a multi-agent setting hadproven challenging. One approach used was to hierarchically decomposethe problem into a global planner, which may omit motion constraints,and a local reactive component that locally modifies the trajectory toincorporate all relevant constraints. The local reactive component isone topic of the following paragraphs, and this description teaches areal-time reactive local motion planner that takes into account motionconstraints, static obstacles, and other flying vehicles or agents.Further, the important case where other agents/vehicles are themselvesdecision-making vehicles is addressed with the shared control method(and system).

Among the inventors contributions provided in their shared controlmethod is a derivation and evaluation of three methods for localplanning in 3D environments or air spaces (e.g., based on recentextensions of the VO concept). A first method is a distributed convexoptimization in which a quadratic cost function is minimized subject tolinear and quadratic constraints. Only knowledge of relative position,velocity, and size of neighboring agents/vehicles is utilized. A secondmethod for local planning uses a centralized convex optimization inwhich a joint quadratic cost function that is minimized subject toquadratic and linear constraints. This variant scales well with thenumber of agents and presents real-time capability but may provide, insome cases, a sub-optimal but still useful solution. The third methoduses a centralized non-convex optimization in which a joint quadraticcost function is minimized subject to linear, quadratic, and non-convexconstraints. This optimization may be formulated as a mixed integerquadratic program, and this variant explores the entire solution spacebut can, in some situations, scale poorly with the number ofagents/vehicles. Each of the optimization methods can: (1) characterizeagents as decision making agents that collaborate to achieve collisionavoidance; (2) incorporate motion continuity constraints; and (3) handleheterogeneous groups of agents. The following description also providesa complexity analysis and a discussion of the advantages anddisadvantages of each optimization method.

The control problem can be thought of as a consideration of a group of nagents freely moving in 3D space. For each agent, i, p_(i) ∈

³ denotes its position, v_(i)={dot over (p)}_(i) its velocity, anda_(i)={umlaut over (p)}_(i) its acceleration. The goal is to compute foreach such agent, at high frequency (e.g., 10 Hz or the like), a localmotion that respects the kinematics and dynamic constraints of the egoagent or aerial vehicle. The local motion should also be collision freewith respect to neighboring agents or aerial vehicles in the 3D spacefor a short time horizon (e.g., for 2 to 10 or more seconds), which maybe thought of as providing local motion that provides collisionavoidance.

Two scenarios are discussed herein: (1) a centralized one where acentral unit computes local motions for all agents simultaneously and(2) a distributed one where each agent or vehicle computes independentlyits local motion. For the second case, no communication between theagents is required, but each agent is able to maintain an estimate ofthe position and velocity of its neighbors. For the distributed case,neighboring agents are labeled as dynamic obstacles or decision-makingagents, which employ an algorithm that is identical to that in the egoagent or vehicle. The local motion planning problem is first formulatedas an efficient convex optimization (either centralized or distributed),followed by a non-convex optimization (centralized) that trades offefficiency for richness of the solution space.

The shape of each agent or vehicle is modeled by an enveloping cylinderof radius, r_(i), and height, 2h_(i), centered at the agent or vehicle.The local motion planning framework is applied to quadrotor helicopters,which are useful in many applications due to their agility andmechanical simplicity. Implementation details, including an appropriatecontrol framework, are described in the coming paragraphs followed bydiscussion of extensive experimental results using up to four quadrotorsalong with a human moving through the 3D space.

As an overview to local motion planning, it may be useful to firstintroduce the terminology used to describe motion planning and thenfollow this with a definition of the local trajectories and an overviewof the optimization problem solved to obtain collision-free motions.With regard to terminology, FIGS. 13A and 13B illustrate schemas 1310,1320 of local motion planning including examples of candidate localtrajectories and examples of a local trajectory, respectively, for anagent 1330 (e.g., a quadrotor helicopter).

A “global trajectory” is a trajectory between two configurationsembedded in a cost field with simplified dynamics and constraints.” A“preferred velocity” is an ideal velocity computed by a guidance systemto track the global velocity, and it may be denoted as ū ∈

³. A “local trajectory” is the trajectory executed by the agent orvehicle for a short time horizon and is selected from a set of candidatelocal trajectories. A “candidate local trajectory” is one that respectsthe kinematics and dynamic constraints of the agent or vehicle and iscomputed from a candidate reference trajectory via a bijective mappingor the like. A “candidate reference trajectory” is a trajectory given bya constant velocity, u ∈

³, that indicates the target direction and velocity of the agent oraerial vehicle. An “optimal reference trajectory” is a trajectory givenby the optimal reference velocity, u* ∈

³, selected from the set of candidate reference velocities and whichminimizes the deviation to ū ∈

³. Further, this trajectory defines the local trajectory.

One focus of the present description is on local motion planning using apreferred velocity, ū, computed by a guidance or control system in orderto track a global trajectory. An optimal reference trajectory iscomputed such that its associated local trajectory is collision free.This provides a parameterization of local motions given by u and allowsfor an efficient optimization in candidate reference velocity space,

³, to achieve collision-free motions.

Each candidate reference trajectory is that of an omni-directional agentmoving at a constant velocity, u, and starting at the current position,p^(k), of the agent as shown by:

P _(ref)(t)=p ^(k) +u(t−t ^(k)), t ∈ [t ^(k),∞)   Eq. (18)

A trajectory tracking controller, respecting the kinematics and dynamicconstraints of the agent, q({tilde over (t)})=f(z^(k),u,{tilde over(t)}) is considered, continuous in the initial state z^(k)=[p^(k),{dotover (p)}^(k),{umlaut over (p)}^(k), . . . ] of the agent and convergingto the candidate reference trajectory. This defines the candidate localtrajectories given by q({tilde over (t)}) and a bijective mapping (for agiven z^(k)) from candidate reference velocities, u.

As an overview of the optimization problem, a collision-free localtrajectory (described above and parameterized by u*) is obtained via anoptimization in candidate reference velocity space where the deviationto the preferred velocity, ū, is minimized. In the followingdescription, two alternative formulations are presented as a centralizedoptimization and a distributed optimization, formulated as a convexoptimization with quadratic cost and linearized constraints for thefollowing: (a) Motion Continuity: The position error between thecandidate reference trajectory and the local trajectory should be belowa limit, ε, to guarantee that the local trajectory is collision free ifthe candidate reference trajectory is. Continuity in position, velocity,acceleration, and further derivatives (optional) is guaranteed byconstruction; and (2) Collision Avoidance: The candidate referencevelocities leading to a collision are described by the velocityobstacle, a cone in relative candidate reference velocity space foragents of ε-enlarged radius. Consider C_(i) the set of collisionavoidance constraints for an agent, i, with respect to its neighbors andC=∪_(i∈A)C_(i).

These non-convex constraints can be linearized leading to a convexoptimization with quadratic cost and linear quadratic constraints. Theoptimization problem is formulated below, and the convex optimization isdescribed for the case of heterogeneous groups of agents of unknowndynamics. Further, an algorithm is described to solve the originalnon-convex optimization along with an extension for homogeneous groupsof agents (same dynamics and controller). In the centralized case, asingle convex optimization is solved where the optimal referencevelocity of all agents, u*_(1:m)=[u*₁, . . . , u*_(m)], are jointlycomputed. In the distributed case, each agent, i ∈ A, independentlysolves an optimization where its optimal reference velocity, u*_(i), iscomputed. In this case, it can be considered that agents collaborate inthe avoidance and apply the same algorithm, otherwise they are treatedas dynamic obstacles. Only information about the neighbor's currentposition, velocity, and shape is utilized in the optimization.

Now, it may be appropriate to further describe a formulation for localmotion planning, and the following discusses in detail the optimizationcost and the optimization constraints used in local motion planning.With regard to preferred velocity, in each local planning iteration, apreferred velocity, ū, can be computed given the current position andvelocity of the vehicle so as to follow a global plan to achieve eithera goal or a trajectory. With regard to achieving a trajectory, thepreferred velocity, ū, is given by a trajectory tracking controller foromnidirectional agents.

With regard to achieving a goal, the preferred velocity, ū, is given bya proportional controller towards the goal position, g_(i), saturated atthe agent's or vehicle's preferred speed, V>0, and decreasing when inthe neighborhood as shown in the following:

$\begin{matrix}{{\overset{\_}{u}}_{i} = {\overset{\_}{V}{\min \left( {1,\frac{{g^{i} - p^{i}}}{K_{\overset{\_}{v}}}} \right)}\frac{g_{i} - p_{i}}{{g_{i} - p_{i}}}}} & {{Eq}.\mspace{14mu} (19)}\end{matrix}$

where K _(v) >0 is the distance to the goal from which the preferredvelocity is reduced linearly. Alternatively, it can be given by an LQRcontroller.

With regard to repulsive velocity, formulating the local motion planningas an optimization where avoidance constraints are given by VelocityObstacles produces collision-free motions but typically reducesinter-agent distance to zero. Although optimal, this property can leadto unsafe motions in real scenarios with sensor noise. To appropriatelymaintain a minimum inter-agent distance, the preferred velocity, ū, iscorrected by adding a repulsive velocity,

, given by a vector field such as those shown in one of the two examplesof repulsive velocities shown in the graph 1400 of FIG. 14 and thatpushes agents apart from each other when at a certain distance. Exampleequations for repulsive velocity that may be utilized in the sharedcontrol method are provided below.

The optimization cost can be given by two parts, with the first onebeing a regularizing term penalizing changes in reference velocity andthe second one being chosen for minimizing the deviation to thepreferred velocity, ū_(i), corrected by the repulsive velocity,

_(i). For example, it has been shown that pedestrians prefer to maintaina constant velocity in order to minimize energy, and, hence, preferencewas given to turning instead of changing speed. Furthermore, penalizingchanges in speed leads to a reduction of deadlock situations. This ideais formalized as an elliptical cost, higher in the direction parallel toū_(i)+

_(i), and lower perpendicular to it. One can let λ>0 represent therelative weight and then define L=diag(λ,1,1) and D_(i) the rotationmatrix of the transformation onto the desired reference frame.

In the distributed case, the quadratic cost can be given by:

C(u _(i)):=K _(o) ∥u _(i) −v _(i)∥² +∥L ^(1/2) D _(i)(u _(i)−(ū _(i)+

_(i)))∥²   Eq. (20)

where, with the exception of the optimization variables, u_(i), thevalues of all other variables are given for the current time instanceand K_(o) is a design constant.

In the centralized case, the quadratic cost can be given as:

C(u _(1:m)):=K _(o)Σ_(i=1) ^(m) w _(i) ∥u _(i) −v _(i)∥²+Σ_(i=1) ^(m) w_(i) ∥L ^(1/2) D ^(i)(u _(i)−(ū _(i)+

_(i))∥²   Eq. (21)

where w _(i) represents relative weights between different agents totake into account avoidance preference between agents, which can beinterpreted as aggressive (high w _(i)) versus shy (low) w _(i))behavior.

Turning now to motion continuity constraints, in order to guaranteecollision-free motions, it can be guaranteed that each agent remainswithin ε_(i) of its reference trajectory, withε_(i)<min_(j)∥p_(j)−p_(i)∥/2. So, a first constraint (“Constraint 1”)for motion continuity can be stated: for a maximum tracking error,ε_(i), and current state, z_(i)=[p_(i),{dot over (p)}_(i),{umlaut over(p)}_(i)], the set of candidate reference velocities, u_(i), that can beachieved with position error lower than the maximum tracking error,ε_(i), is given by:

R _(i) =R(z _(i),ε_(i))={u _(i)|∥(p _(i) +{tilde over (t)}u _(i))−f(z_(i) ,u _(i) ,{tilde over (t)})∥≦ε_(i) ,∀{tilde over (t)}>0}  Eq. (22)

For arbitrary f(z_(i),u_(i),{tilde over (t)}), the set Ri can beprecomputed. For quadrotors freely moving in 3D space, an LQR controllercan be used as the function f(z_(i),u_(i),{tilde over (t)}). In thepresent description, poison control and local trajectory are decoupledand f(z_(i),u_(i),{tilde over (t)}) is given by Eq. (40). If the agentswere omnidirectional, the set Ri would be given by a sphere centered atzero and of radii v_(max) (no continuity in velocity). For the case ofstudy of this description, given a maximum tracking error, ε_(i), and aninitial state, z_(i), the limits can be obtained from the computed datawith:

$\begin{matrix}{{\delta \; R_{i,ɛ_{i}}} = \left\{ {{u_{i}_{i > 0}^{\max}{{\left( {p_{i} + {\overset{\sim}{t}u_{i}}} \right) - {f\left( {z_{i},u_{i},\overset{\sim}{t}} \right)}}}} = ɛ_{i}} \right\}} & {{Eq}.\mspace{14mu} (23)}\end{matrix}$

To reduce computational complexity, δR_(i,ε) _(i) is then approximatedby the largest ellipse (quadratic constraint of the form u_(i)^(T)Q_(i)u_(i)+1_(i)·u_(i)≦a_(i), with Q_(i) ∈

^(3×3) positive semidefinite, l_(i) ∈

) fully contained inside δR_(i,ε) _(i) .

With regard to collision avoidance constraints, each agent or vehicleshould avoid colliding with static obstacles, other controlled agents orvehicles, and dynamic obstacles. A maximum time horizon, τ, can beconsidered for which collision-free motion is guaranteed. A staticobstacle is given by a region O ⊂

³. The vehicles or agents are modeled by an enclosing vertical cylinder.The disk of radius r centered at c is denoted by D_(c,r) ⊂

², and the cylinder of radius r and height 2h centered at c is denotedby C_(c,r,h) ⊂

³.

The collision avoidance constraints are computed with respect to anagent or vehicle following the reference trajectory. Since the agentsare not able to perfectly track the reference trajectory, theirenveloping cylinder should be enlarged by the maximum tracking error,ε_(i). For simplicity, the following notation is used:r_(i):=r_(i)+ε_(i) and h_(i):=h_(i)+ε_(i) for the enlarged radius andthe cylinder height with p_(i) being the current position of an agent,i. Then a second constraint (“Constraint 2”) regarding avoidance ofstatic obstacles can be stated as: for every agent i ∈ A and neighboringobstacle O, the constraint is given by the reference velocities forwhich the intersection between O and the agent is empty for all futuretime instances, i.e., p_(i)+u_(i){tilde over (t)} ∉ (O ⊕ C_(0,r) _(i),h_(i)),∀{tilde over (t)} ∈ [0,τ].

This constraint can be given by a cone in the 3D candidate referencevelocity space generated by O ⊕ C_(0,r) _(i) ,h_(i)−p_(i) and truncatedat (O ⊕ C_(0,r) _(i) _(,hi)−p_(i))/τ. An example of this constraint fora rectangular object is shown in the fly space or graph 1500 of FIG. 15.Boundary wall constraints are directly added, given byn·u_(i)≦d(wall,p_(i))/τ, with n being the normal vector to the wall andd(wall,p_(i)) being the distance to it.

A third constraint (“Constraint 3”) for inter-agent collision avoidancecan be stated: for every pair of neighboring agents i,j ∈ A, theconstraint is given by the relative reference velocities for which theagents' enveloping shape does not intersect within the time horizon,i.e., u_(i)−u_(j) ∉ VO_(ij) ^(τ)=∪_({tilde over (t)}=0) ^(τ)((C_(p) _(j)_(,r) _(j) _(,h) _(j) ⊕ C_(−p) _(i) _(,r) _(i) _(,h) _(i) )/{tilde over(t)}),∀{tilde over (t)} ∈ [0,τ]. For cylindrical-shaped agents, theconstraint can be separated in the horizontal plane ∥p_(i) ^(H)−p_(j)^(H)+(u_(i) ^(H)−u_(j) ^(H)){tilde over (t)}∥≧r_(i)+r_(j), and thevertical component |p_(i) ³−p_(j) ³+(u_(i) ³−u_(j) ³){tilde over(t)}|≧h_(i)+h_(j) for all the {tilde over (t)} ∈ [0,τ]. The constraintis a truncated cone as shown in the graphs 1610, 1620, and 1630 of FIGS.16A-16C (particularly in graph 1610), with graph 1610 illustrating avelocity obstacle in relative velocity, graph 1620 illustratingprojection onto the horizontal plane, and graph 1630 showing projectiononto a vertical plane.

Turning now to approximation by linear constraints, since the volume ofreference velocities leading to a collision is convex, theaforementioned collision avoidance constraints are non-convex. To obtaina convex optimization, they can be linearized. For increased controlover the avoidance behavior and topology, each non-convex constraint canbe approximated by five linear constraints representing to the right, tothe left, over and under the obstacle or another agent, and a head-onmaneuver, which remains collision-free up to t=τ. In particular, for thefeasible space

³\VO_(ij) ^(τ) consider five (l ∈ [1,5]) linear constraints(half-spaces) H_(ij) ^(l) of the form n_(ij) ^(l)(u_(i)−u_(j))≦b_(ij)^(l), with n_(ij) ^(l) ∈

³ and b_(ij) ^(l) ∈

, and verifying that if u_(i)−u_(j) satisfies any of the linearconstraints, it is feasible with respect to the original constraintthat:

∪_(l=1) ⁵H_(ij) ^(l) ⊂

³\VO_(ij) ^(τ)  Eq. (23)

An example of this process is provided in FIGS. 16A-16C, and theequations of the linear constraints are provided below. To linearize theoriginal constraint, only one of the linear constraints is typicallyselected and added to the convex optimization.

Now, with regard to selection of a linear constraint, each collisionavoidance constraint can be linearized by selecting one of the fivelinear constraints. First, one may choose a fixed side for avoidance. Ifagents are moving towards each other, (v_(ij)·p_(ij)<0), one may wish toavoid on a predefined side such as on the left (l=1). If agents are notmoving towards each other, the constraint perpendicular to the apex ofthe cone (l=5) can be selected to maximize maneuverability. Second, thelinear constraint chosen may be maximum constraint satisfaction for thecurrent relative velocity:

$\begin{matrix}{\arg \; \min} \\l\end{matrix}{\left( {{n_{ij}^{l} \cdot \left( {v_{i} - v_{j}} \right)} - b_{ij}^{l}} \right).}$

This selection maximizes the feasible area of the optimization, whentaking into account the motion continuity constraints discussed above.

A third choice may be maximum constraint satisfaction for the preferredvelocity:

$\begin{matrix}{\begin{matrix}{argmin} \\l\end{matrix}\left( {{n_{ij}^{l} \cdot \left( {{\overset{\_}{u}}_{i} - {\overset{\_}{u}}_{j}} \right)} - b_{ij}^{l}} \right)} & (1)\end{matrix}$

if centralized and

$\begin{matrix}{\begin{matrix}{argmin} \\l\end{matrix}\left( {{n_{ij}^{i} \cdot \left( {{\overset{\_}{u}}_{i} - v_{j}} \right)} - b_{ij}^{l}} \right)} & (2)\end{matrix}$

if distributed. This selection may provide faster progress towards thegoal position, but the optimization may become quickly infeasible if theagent greatly deviates from its preferred trajectory. The first choiceor option provides the best coordination results as it incorporates asocial rule. The second choice or option maximizes the feasible area ofthe optimization, and the third choice or option may provide fasterconvergence to the ideal trajectory. An experimental evaluation of theseoptions is given below. Without loss of generality, considern_(ij)·u_(ij)≦b_(ij) the selected linearization of

³\VO_(ij) ^(τ), which can be directly added in the centralizedoptimization of Algorithm 1 discussed below.

With regard to partition, in the distributed case, the referencevelocity of an agent, j, is typically unknown, and only its currentvelocity, v_(i), can be inferred. With the assumption that every agentfollows the same optimization algorithm, a partition should be foundsuch that if both agents select new velocities independently theirrelative reference velocity, u_(i)−u_(j), is collision free. The idea ofreciprocal avoidance can be followed. The change in velocity is denotedby Δv_(i)=u_(i)−v_(i) and the relative change of velocity byΔv_(ij)=Δv_(i)−Δv_(j). In the distributed case, all agents areconsidered as independent decision makers solving their independentoptimizations. To globally maintain the constraint satisfaction andavoid collisions, an assumption on an agent's, j's, velocity can bemade.

Variable sharing of avoidance effort might be considered in some casesleading to Δv_(i)=λΔv_(ij) with the assumption thatΔv_(j)=−(1−λ)Δv_(ij). For collaborative agents that equally share theavoidance effort, λ=0.5. If it is considered that an agent, j, ignoresanother agent, i, and continues with its current velocity, then λ=1 andfull avoidance is performed by the agent, i. With the assumption thatboth agents share the avoidance effort (change in velocity) for λ ∈[0,1], the linear constraint can be written as:

$\begin{matrix}\begin{matrix}{{n_{ij} \cdot u_{ij}} = {n_{ij} \cdot \left( {u_{i} - v_{j} - {\Delta \; v_{j}}} \right)}} \\{= {n_{ij} \cdot \left( {u_{i} - v_{j} + {\left( {1 - \lambda} \right)\Delta \; v_{ij}}} \right.}} \\{= {n_{ij} \cdot \left( {u_{i} - v_{j} + {\left( {1 - \lambda} \right){\left( {u_{i} - v_{i}} \right)/\lambda}}} \right)}} \\{= {{\frac{n_{ij}}{\lambda} \cdot \left( {{{u_{i}\left( {1 - \lambda} \right)}v_{i}} - {\lambda \; v_{j}}} \right)} \leq b_{ij}}}\end{matrix} & {{Eq}.\mspace{14mu} (24)}\end{matrix}$

which leads to:

n _(ij) ·u _(i) ≦b _(i) =λb _(ij) +n _(ij)·((1−λ)v _(i) +λv _(j)   Eq.(25)

which is a fair partition of the velocity space for both agents andprovides avoidance guarantees.

At this point, it may be useful to provide exemplary algorithms for usein convex optimization. To achieve low computational time, theinherently non-convex optimization can be approximated by a convexoptimization with quadratic cost and linear and quadratic constraints.The following two algorithms can be used in this regard with one beingfor use in the centralized implementation and one in the distributedimplementation.

The first algorithm (“Algorithm 1”) for centralized convex optimizationcan be stated as: a single convex optimization is solved where theoptimal reference velocity of all agents, u*_(1:m)=[u*₁, . . . ,u*_(m)], are jointly computed as:

$\begin{matrix}{{u_{1:m}^{*} = {\begin{matrix}{argmin} \\u_{1:m}\end{matrix}{C\left( u_{1:m} \right)}}}{{{s.t.\mspace{14mu} {u_{i}}} \leq v_{\max}},{\forall{i \in A}}}{{{quadratic}\mspace{14mu} {Constraint}\mspace{14mu} 1},{\forall{i \in A}}}{{{linearized}\mspace{14mu} {Constraint}{\mspace{11mu} \;}2},{\forall{i \in A}}}{{{linearized}\mspace{14mu} {Constraint}{\mspace{11mu} \;}3},{{of}\mspace{14mu} {the}\mspace{14mu} {form}}}{{{n_{ij} \cdot \left( {u_{i} - u_{j}} \right)} \leq b_{ij}},{\forall i},{j \in {A\mspace{14mu} {neighbors}}}}} & {{Eq}.(26)}\end{matrix}$

where the optimization by Eq. (21).

The second algorithm (“Algorithm 2”) for distributed convex optimizationcan be states as: each agent, i ∈ A, independently solves anoptimization where its optimal reference velocity, u*_(i), is computedas:

$\begin{matrix}{{u_{i}^{*} = {\begin{matrix}{\arg \; \min} \\u_{i}\end{matrix}{C\left( u_{i} \right)}}}{{{s.t.\mspace{11mu} {{u_{i}}}} \leq v_{\max}},{{quadratic}\mspace{14mu} {Constraint}\mspace{14mu} 1},{{agent}\mspace{14mu} i}}\mspace{14mu} {{{linearized}\mspace{14mu} {Constraint}\mspace{14mu} 2},{{agent}\mspace{14mu} i}}\mspace{14mu} {{{linearized}\mspace{14mu} {Constraint}\mspace{14mu} 3},{{of}\mspace{14mu} {the}\mspace{14mu} {from}}}\mspace{14mu} {{{n_{ij} \cdot u_{i}} \leq b_{i}},{\forall{j \in {A\mspace{14mu} {neighbor}\mspace{14mu} {of}\mspace{14mu} i}}}}} & {{Eq}.\mspace{11mu} (27)}\end{matrix}$

where the optimization cost is given by Eq. (20). These 2m and2-dimensional quadratic optimizations with linear and quadraticconstraints can be solved efficiently using presently available solvers.

If the optimization is feasible, a solution is found that guaranteescollision-free motion up to a predefined time horizon, τ. If theoptimization is infeasible, no solution exists for the linearizedproblem. In this case, the involved agents drive or fly their lastfeasible trajectory (obtained at time t_(f)) with a timere-parameterization given by:

γ(t)=(−t _(f) ²+(2τ+2t _(f))t−t ²)/(2τ), t ∈ [t ^(f) , t ^(f)+τ], γ(t)=t_(f)+τ/2, t<t _(f)+τ  Eq. (28)

This time re-parameterization guarantees that the involved agents reacha stop on their previously computed paths within the time horizon of thecollision avoidance algorithm. Since this computation is performed at ahigh frequency, each individual agent is able to adapt to changingsituations.

With these two convex optimization algorithms in mind, it may now beuseful to discuss several theoretical guarantees. A first theoreticalguarantee or remark pertains to computational complexity and can bestated as follows: linear in the number of variables and constraintsalthough the centralized is of higher complexity. If distributed, foreach agent, the optimization utilizes three variables, one quadraticconstrain, and a maximum of m linear constraints for collision avoidance(in practice, limited to a constant value, K_(c)). If centralized, theoptimization uses 3n variables, n quadratic constraints, and less than

$\frac{n\left( {n - 1} \right)}{2} + {n\left( {m - n} \right)}$

linear constraints (usually limited to nk_(c)).

A second theoretical guarantee or remark pertains to safety guaranteesand can be stated as: safety is preserved in normal operation. Iffeasible, collision-free motion is guaranteed for the local trajectoryup to a particular time, τ (optimal reference trajectory iscollision-free for agents of radii enlarged by the maximum trackingerror, ε, and agents stay within ε of it) with the assumption that allinteracting agents either continue with their previous velocity orcompute a new one following the same algorithm and assumptions. If onelets p_(i)(t) denote the position of an agent, i, at time t≧t^(k) andrecalls that {tilde over (t)}=t−t^(k), then:

∥p _(i)(t)−p _(j)(t)∥=∥f(z _(i) ^(k) ,u _(i) ,{tilde over (t)})−f(z _(j)^(k) ,u _(j) ,{tilde over (t)})∥≧∥(p _(i) ^(k) +u _(i) {tilde over(t)})−(p _(j) ^(k) +u _(j) {tilde over (t)})∥−ε_(i)−ε_(j) ≧r _(i)+ε_(i)+r _(j)+ε_(j)−ε_(i)−ε_(j) =r _(i) +r _(j)   Eq. (29)

where the first inequality holds from the triangular inequality andConstraint 1 (u_(i) ∈ R_(i) and u_(j) ∈ R_(j)). The second inequalityholds from Constraint 3 (u_(i)−u_(j) ∉ VO_(ij) ^(τ)).

If infeasible, no collision-free solution exists that respects all theconstraints. If the time horizon is longer than the required time tostop, passive safety is preserved if the last feasible trajectories ofall agents are driven with a time re-parameterization to reach stopbefore a collision arises (see Eq. 28). This time re-parameterizationimplies a slowdown of the agent, which in turn may render theoptimization feasible in a later time step. Since this computation isperformed at a high frequency, each individual agent can adapt tochanging situations.

A third theoretical guarantee or remark pertains to infeasibility andcan be stated as: the optimization can be infeasible due to severalcauses. First, there may not be enough time to find the solution withinthe allocated time. Second, there may be differences between the modeland the real vehicle. Third, there may be noise in localization andestimation of the agents' states. Fourth, the optimization can beinfeasible due to the limited local planning horizon together with anover simplification of motion capabilities by reducing them to the setof local motions. Fifth, if distributed and given the use of pair-wisepartitions of velocity, space with either the assumption of equal effortin the avoidance or constant speed, the optimization may be infeasiblewhen not all world constraints are taken into account for theneighboring agents. Thus, an agent may have conflicting partitions withrespect to different neighbors rendering its optimization infeasible.

A fourth theoretical guarantee or remark pertains to deadlock-freeguarantees and can be stated as: as for other local methods, deadlocksmay still appear. With this in mind, a global planner for guidance maybe useful in complex scenarios.

Now, it may be useful to provide an exemplary algorithm for use innon-convex optimization. Earlier, a method for local motion planning wasdescribed for heterogeneous groups of aerial vehicles (or agents), whichwas formulated as a convex optimization problem. The following presentsan extension to a non-convex optimization where the global optimum canbe found but at an increased computational cost. As taught earlier inthis description, each non-convex constraint can be approximated by fivelinear constraints of which only one is introduced in the optimization,which leads to a local optimum. The full solution space can be exploredby the addition of binary variables and reformulating the problem as amixed integer quadratic program (MIQP).

One binary variable, ξ_(c) ^(l)={0,1}, is added for each linearconstraint, l ∈ [1:5], approximation of a nonconvex constraint, c ∈ C.Only one out of the five linear constraints is active, thus Σ_(l=1)⁵ξ_(c) ^(l)=4,∀c ∈ C. An algorithm (“Algorithm 3”) for non-convexoptimization (or for centralized mixed-integer optimization) can now beformulated. A single MIQP optimization is solved where the optimalreference velocity of all agents, as well as the active constraints,ξ=∪_(l∈[1,5],c∈C)ξ_(c) ^(l), are jointly computed. A vector, f _(ξ)^(T), can be defined with (optional) weights for each binary variable,for instance, to give preference to avoidance on one side, and thenAlgorithm 3 can presented as:

argmin${C\left( u_{1\text{:}m} \right)} + {f\frac{T}{\xi}\overset{\_}{\xi}}$u_(1:m)   s.t. ∥u_(i)∥ ≦ u_(max), ∀i ε A quadratic Constraint 1, ∀i ε Alinear Constraints 2, of the form  n_(i) ^(l) · u_(i) − Nξ_(i) ^(l) ≦b_(i) ^(l), ∀i ε A, ∀l ε [1,5] linear Constraints 3, of the form  n_(ij) ^(l) · (u_(i) − u_(j)) − Nξ_(ij) ^(l) ≦ b_(ij) ^(l),    ∀i, j εA, neighbors, ∀l ε [1,5] Σ_(i) ⁵ ξ_(c) ^(l) = 4, ∀c ε C     Eq.(30)where N>>0 is a large constant of arbitrary value. The linear costvector f _(ξ) is a zero vector if no preference between linearconstraints for each VO exists. Different weights can be added for eachlinear constraint to include preference for a particular avoidance side.

This optimization can be solved via branch and bound using state of theart solvers. Although the number of variables and constraints can bebounded to be linear with the number of agents, the number of branchesto be explored increases exponentially. In practice, a bound in theresolution time or number of explored branches (avoidance topologies)should be set. A distributed MIQP optimization can also be formulated,but collision avoidance guarantees would be lost since this can lead toa disagreement in the avoidance side and reciprocal dances appear.

With all the above teaching in mind, a control framework can bepresented that is useful with aerial vehicles, and the inventorsimplemented and experimentally evaluated the control framework forproviding shared control over quadrotor helicopters as the aerialvehicles. Particularly, FIG. 17 illustrates a control framework orcontroller 1700 that was implemented to provide control over a quadrotorvehicle 1710, and, to this end, the controller 1700 includes a number ofsubmodules (e.g., programs executed by a processor of a central unit ina centralized implementation) 1720-1790 whose functionality and/oroperations are described in the following paragraphs.

Motion planning may be divided into global planning and local planning.With regard to global planning, in complex environments, a globalplanner may be desirable for convergence, but in this discussion a fixedtrajectory or goal position may be considered or used (as shown at1720). With regard to local planning, the controller 1700 may compute(e.g., with a 10 Hz frequency) a collision-free local motion given adesired global trajectory or a goal position. To this end, a guidancesystem 1730 may output a preferred velocity, ū, given a desired globaltrajectory or a goal position and the current state of the vehicle.Additionally, a collision avoidance module 1740 receives the preferredvelocity, ū, and computes the optimal reference velocity, u*, i.e., theclosest collision-free candidate reference velocity that satisfies themotion continuity constraints.

With regard to control, a position controller 1760 controls the positionof the quadrotor helicopter (or other aerial vehicle) to the localtrajectory and runs at a desired frequency such as 50 Hz offboard in areal-time computer. The local trajectory interpreter 1750 receives theoptimal reference velocity, u*, at a number of time instances, t^(k).The module 1750 then outputs the control states, q, {dot over (q)},{umlaut over (q)}, to satisfy motion continuity at a particular time,and asymptotic convergence to the optimal reference trajectory as may begiven by p^(k)+u*{tilde over (t)}. The position controller 1760 receivessetpoints in position, q, velocity, {dot over (q)}, and acceleration,{umlaut over (q)}, and outputs the desired force, f.

The vehicle 1710 may be configured to provide a processor, memory, andsoftware to provide a low-level controller that runs onboard (e.g., a200 Hz or the like) and provides accurate attitude control at highfrequency. The low-level controller may abstract the nonlinear quadrotormodel as a point-mass for the high-level control. It may be composed ofthe following three submodules: (1) an attitude controller 1780 thatreceives a desired force, f (collective thrust and desired orientation)and outputs desired angular rates, w _(x), w _(y), w _(z); (2) a bodyangular rate controller that receives the desired body angular rates,and collective thrust, T, and provides an output in the form of arotation speed setpoint for each of the motors [ω₁, . . . , ω₄]; and (3)a motor controller that receives the desired rotation speeds of themotors [ω₁, . . . , ω₄], with some implementations of vehicle 1710 usingoff-the-shelf motor controllers to interface the four motors of thequadrotor.

A quadrotor is a highly dynamic vehicle. To account for delays, thestate used for local planning may not be the current state of thevehicle but, instead, a predicted one (e.g., as may be provided byforward simulation of the previous local trajectory) at the time whenthe new local trajectory is sent. A quadrotor is an under-actuatedsystem in which translation and rotation are coupled. Its orientation(attitude) is defined by the rotation matrix, R_(I) ^(B)=[e_(x), e_(y),e_(z)] ∈ SO(3), which describes the transformation from the inertialcoordinates frame, I, to the body fixed coordinate frame, B. Thefollowing paragraphs provide an overview of the model and control.

With regard to a quadrotor model (vehicle model), each of the fourmotors of the quadrotor has an angular speed, w_(i), and produces aforce, F_(i)=c_(t)w_(i) ², and a moment, M_(i)=c_(y)w_(i) ², where c_(t)and c_(y) are model-specific constants. Since the motor dynamics arefast, it is assumed that the desired force, F_(i), is reachedinstantaneously. The control input, o=[T, o₁], with o₁=[M_(x), M_(y),M_(z)], T being the total thrust, and M_(x), M_(y), and M_(z) being themoments around the body axis, is given by:

$\begin{matrix}{o = {\begin{bmatrix}T \\o_{1}\end{bmatrix}\underset{\underset{U}{}}{\begin{bmatrix}c_{t} & c_{t} & c_{t} & c_{t} \\0 & {c_{a}1} & 0 & {{- c_{a}}1} \\{{- c_{a}}1} & 0 & {c_{a}1} & 0 \\c_{y} & {- c_{y}} & c_{y} & {- c_{y}}\end{bmatrix}}\underset{\underset{w_{m}}{}}{\begin{bmatrix}w_{1}^{2} \\w_{2}^{2} \\w_{3}^{2} \\w_{4}^{2}\end{bmatrix}}}} & {{Eq}.\mspace{14mu} (31)}\end{matrix}$

where l is the length from the center of gravity of the quadrotor to therotor axis of the motor and c_(a) is a model constant.

The angular velocity, w, and its associated skew anti-symmetric matrix,w, are denoted by:

$\begin{matrix}{{w:=\begin{bmatrix}0 & {- w_{z}} & w_{y} \\w_{z} & 0 & {- w_{x}} \\{- w_{y}} & w_{z} & 0\end{bmatrix}},{w:=\begin{bmatrix}w_{x} \\w_{y} \\w_{z}\end{bmatrix}}} & {{Eq}.\mspace{14mu} (32)}\end{matrix}$

The angular acceleration around the center of gravity is given by:

{dot over (w)}=J ⁻¹ [−w×Jw+o ₁]  Eq. (33)

with J being the moment of inertia matrix reference to the center ofmass along the body axes of the quadrotor. If R_(B) ^(I)(R_(I) ^(B))⁻¹denotes the transformation from the body to the inertial frame, therotational velocity of the quadrotor's fixed body frame with respect tothe inertial frame can be given by {dot over (R)}_(B) ^(I)=wR_(B) ^(I).

A point-mass model can be used for the position dynamics. Theaccelerations of the quadrotor's point-mass in the inertial frame,{umlaut over (p)}_(I), are given by:

$\begin{matrix}{{\overset{¨}{p}}_{I} = {{R_{B}^{I}\begin{bmatrix}0 & 0 & {- \frac{T}{m}}\end{bmatrix}}^{T} + \begin{bmatrix}0 & 0 & g\end{bmatrix}^{T}}} & {{Eq}.\mspace{14mu} (34)}\end{matrix}$

where two forces are applied, gravity in the z direction in the inertialframe is given by F_(G)=mg, and T is the thrust in the negative zdirection in the quadrotor body-frame.

With regard to attitude control, an approach may be followed thatinvolves directly converting differences of rotation matrices into bodyangular rates. A desired force setpoint, F, and a desired yaw angle, ψ,given by a high-level controller are transformed to a desired rotationmatrix R=[ē_(x), ē_(y), ē_(z)] with

$\begin{matrix}{{{\overset{\_}{e}}_{z} = \frac{\overset{\_}{F}}{\overset{\_}{F}}},{{\overset{\_}{e}}_{y} = \frac{{\overset{\_}{x}}_{\psi} \times {\overset{\_}{e}}_{z}}{{{\overset{\_}{x}}_{\psi} \times {\overset{\_}{e}}_{z}}}},{{\overset{\_}{e}}_{x} = {{\overset{\_}{e}}_{y} \times {\overset{\_}{e}}_{z}}}} & {{Eq}.\mspace{14mu} (35)}\end{matrix}$

and x _(ψ)=[cos ψ, sin ψ, 0]^(T) is a vector pointing in the desired yawdirection. The desired body angular rates can then be controlled with astandard PD controller and given by:

$\begin{matrix}{\overset{\_}{w} = {f\left( {\frac{1}{2}\left( {{{\overset{\_}{R}}_{I}^{B}{\hat{R}}_{B}^{I}} - {{\hat{R}}_{I}^{B}{\overset{\_}{R}}_{B}^{I}}} \right)} \right)}} & {{Eq}.\mspace{14mu} (36)}\end{matrix}$

where {circumflex over (R)}** indicates the estimated rotation matrices.

Now, with regard to position control, according to the double integratordynamics of Eq. (34), the discrete LTI model can be written as:

X _(k+1) =A _(χk) +B _(u) y=C _(χk)   Eq. (37)

with state vector χ:=[p_(x), v_(x), p_(y), v_(y), p_(z), v_(z)]^(T),input vector ρ=[F_(x), F_(y), F_(z)]^(T), and system matrices given as:

${A:={I_{3} \oplus \begin{bmatrix}1 & {t} \\0 & 1\end{bmatrix}}},{B:={I_{3} \oplus \begin{bmatrix}{st} \\{t}\end{bmatrix}}},{C:={I_{3} \oplus \begin{bmatrix}1 & 0\end{bmatrix}}}$

with I being the identity matrix,

${{st} = {\frac{1}{2}{t^{2}}}},$

and ⊕ being the Kronecker product. To have an integral action for theLQR controller, the system given in Eq. (37) is augmented to thediscrete LTI system:

${\overset{\sim}{X}:=\begin{bmatrix}X \\q\end{bmatrix}},{\overset{\sim}{A}:=\begin{bmatrix}A & 0 \\C & I\end{bmatrix}},{\overset{\sim}{B}:=\begin{bmatrix}B \\0\end{bmatrix}},{C = \begin{bmatrix}C & 0\end{bmatrix}}$

The control input, ρ, is then given with the state feedback lawρ=−K_({tilde over (X)}) with K being the solution to the DiscreteAlgebraic Riccati equation (DARE) with state and input cost matrices.

With regard to motion primitives, a point-mass M-order integrator modelcan be used or considered, decoupled in each component:

q ^((M+1))({tilde over (t)})=v   Eq. (38)

Then, one can let v ∈

³ be piecewise continuous and (.)^((M+1)) represent the derivative oforder M+1 with respect to time. The resulting solution q({tilde over(t)}) is C^(M) differentiable at the initial point. For M>0 the statefeedback control law is given by:

v({tilde over (t)})=−a _(M) q ^((M)) − . . . −a ₂ {umlaut over (q)}+a₁(u−{dot over (q)})+a ₀(p ^(k) +u{tilde over (t)}−q)   Eq. (39)

It has been shown that the under-actuated model of a quadrotor can bewritten as a differentially flat system, with its trajectoryparameterized by its position and yaw angle and their derivatives up toforth degree in position and second degree in yaw angle. In thefollowing, the yaw angle is considered constant. Although the methodpresented herein extends to M=4, in order to simplify the formulationsgiven the difficulty of accurately estimating the high order derivativesand their marginal effect, only continuity in acceleration (M=2) isimposed in the following discussion. For M=2 (continuity in initialacceleration), the motion primitive is:

q({tilde over (t)})=p ^(k) +u{tilde over (t)}+Fe ^(−w) ¹^({tilde over (t)})+(({dot over (p)} ^(k) −u+(w ₁ −w ₀)F){tilde over(t)}−F)e ^(−w) ⁰ ^({tilde over (t)})  Eq. (40)

with F=({umlaut over (p)}^(k)+2w₀({dot over (p)}^(k)−v∞))/(w₀−w₁)² andw₀, w₁ being design parameters related to the decay of the initialvelocity and acceleration, which can potentially be different for eachof the three axes.

At this point in the discussion, it may be useful to describe results ofexperiments performed by the inventors with the shared control method ofaerial vehicles. Particularly, experiments were performed both insimulation and with physical quadrotors. Simulation utilized a model ofthe dynamics and enabled experiments for larger groups of vehicles. Asan example, FIG. 18 illustrates with graph 1800 position exchange forten quadrotors in a simulation. The top of graph 1800 shows a horizontal(x-y) view and the bottom of graph 1800 shows a vertical (x-z) view.Vehicle position is depicted for timestamps 0 s:4 s:6 s:8 s:12 s in anarena of size 8 m by 4 m by 8 m. Convergence to the goal configurationwas achieved in about 15 seconds in this simulation. All other resultsprovided herein are for physical quadrotor helicopters.

The experimental space utilized was a rectangular space that wasapproximately 5 m by 5.5 m by 2 m in height. Quadrotor position wasmeasured by an external motion tracking system running at 250 Hz. Eachvehicle was modeled with the following parameters: w₀=3, w₁=3.5,r_(i)=0.35 m, h_(i)=0.5 m, v_(max)=2 m/s, a_(max)=2 m/s², ε=0.1 m, andτ=3 s. The control system 1700 was used to control the quadrotors. Alow-level attitude estimator and controller were run on each quadrotor'sIMU, and the quadrotor was abstracted as a point-mass for the positioncontroller. The position controller was run on a real-time computer andcommunication between the real-time computer and the quadrotor was donewith a 50 Hz UART bridge. The overlying collision avoidance was run in anormal operating system environment and sent a collision-free optimalreference trajectory to the position controller over a communicationchannel. The position controller ensured that the quadrotors stayed ontheir local trajectories as computed by the collision avoidancealgorithm. This hard real-time control structure ensured the stabilityof the overall system and kept the quadrotors on given positionreferences.

The benchmark performance of the system was evaluated for a singlequadrotor running the full framework, including local planning in whichcontinuity up to acceleration was imposed. FIG. 19 illustrates withgraphs 1910, 1920, 1930, and 1940 a single quadrotor tracking a circularmotion given by a sinusoid reference signal in all three positioncomponents. FIG. 19 shows with graph 1910 the projection of the positionp of the quadrotor onto the horizontal plane and shows with graph 1920the projection onto a vertical plane. The reference circular motion isinfeasible due to the space limitation of the experiment room, but thelocal planning successfully avoided colliding with the walls causing theflattened edges in the y component. The graph 1930 shows each componentof the position p with respect to time as a continuous line, while thepoint-mass local trajectory q is shown by a dashed line. Good trackingperformance was observed, although with a small delay due to unmodeledsystem delays and higher order dynamics. A steady-state error is furtherobserved due to the use of an LQR controller without integral part inthe experiments. Steady-state error is greatest in the verticalcomponent, as a thrust calibration is not performed before flight, andits value varies from quadrotor to quadrotor. To handle steady stateerror, continuity s imposed in the local trajectory instead of on thereal state of the quadrotor. This approach adapts well to poorlycalibrated systems, like the one used in the experiments.

FIG. 19 in graph 1940 shows the measured velocity v of the quadrotor asa continuous line and the velocity of its point-mass local trajectory asa dashed line with respect to time. Good tracking performance wasobserved, although the delay was more apparent, especially for abruptchanges in velocity, which may be partially due to the unmodeledhigh-order dynamics of the quadrotor. The smoothness of the velocityprofile can be adjusted by the parameters w₀ and w₁ in Eq. (40), whichcan be tuned for overall good performance.

Position swaps were performed with sets of two and four quadrotors, withabout fifty transitions to different goal configurations (both antipodaland random). The centralized algorithm (Algorithm 1) and the distributedalgorithm (Algorithm 2) were both tested, with the three differentlinearization options for the collision avoidance constraints describedabove. The joint MIQP optimization (Algorithm 3) was also tested, but itproved too slow for real-time performance at 10 Hz in the experimentalimplementation. However, in simulation, this approach provided goodresults.

FIG. 20 shows a representative position exchange for two quadrotors. Ingraphs 2010 and 2020, FIG. 20 shows the projection of their paths on thehorizontal and vertical planes. The slight change in height is due tothe dynamics of the quadrotors. FIG. 20 in graph 2030 shows the velocityprofile for both quadrotors. The preferred speed of the quadrotors is 2m/s, and the position exchange takes a few seconds. Very similarperformance was obtained in the distributed and centralized cases andwith the different linearization options.

Two experiments were performed in which four quadrotors weretransitioned to antipodal positions while also changing height. Twodifferent linearization strategies were employed, with one being theimposition of avoidance and with one being a case when collisionavoidance constraints are linearized with respect to the measuredvelocity. FIG. 21 with graphs 2110, 2120, 2130, 2140, 2150, and 2160 theresults these two experiments. Graphs 2110, 2120, and 2130 illustrateresults of use of the first linearization strategy while graphs 2140,2150, and 2160 illustrate results of use of the second linearizationstrategy. Graphs 2110, 2120, 2140, and 2150 show the projection of theposition of all quadrotors onto the horizontal plane and a verticalplane. Graphs 2130 and 2160 show the velocity profile for allquadrotors. The preferred speed was 2 m/s, and the dimensions of theexperiment room (fly space) only allowed the quadrotors to fly at aheight between 0 and 2 meters. The position exchanges were collisionfree due in part to the local planning algorithm even for this morecomplex case in which the preferred velocity always points towards thegoal positions and, therefore, is not collision free.

For the case when avoidance to the right was imposed, the behavior washighly predictable and smooth, with the rotation of all vehicles to oneside being imposed in the linearization. This shows the control methodprovided good results both in the centralized case and the distributedcase. Although this characteristic is effective for cases of symmetry,in arbitrary configuration, it can lead to longer trajectories. For thecase of linearization with respect to the current velocity, the behaviorcan be different for different runs and emerges from non-deterministiccharacteristics of the motion. It was observed that in this particularexample that two quadrotors chose to pass over each other using thecontrol method. Although this linearization option presents very goodperformance in not too crowded environments, its performance may degradein the case of high symmetry and very crowded scenarios (for example,due to disagreements in avoidance side especially in the distributedcase).

Now, it may be useful to discuss the results of experiments performed totest the method for trajectory tracking, e.g., in the case of a movinggoal. Two representative scenarios are described, and vertical motion isnot displayed in the results because, without loss of generality, motionwas approximately in a horizontal plane. FIG. 22 illustrates with graphs2210, 2220, and 2230 the results of an experiment in which twoquadrotors were tracking intersecting trajectories while avoidingcollision locally. One quadrotor followed a circular motion (see line2211) and one followed a diagonal motion (see line 2213 such that thereare two intersection points in position and time. The one following acircular motion finally transitioned to a rest position. In graph 2210,FIG. 22 shows that both controlled quadrotors often deviate from theirideal trajectories in order to avoid collision. In graphs 2220 and 2230,the velocity plot shows that no deadlock appeared and both quadrotorswere moving at a velocity of about 1.5 m/s except towards the end of theexperiment where the quadrotor following a circular motion approached astatic goal position. The trajectory for the other quadrotor thenbecomes feasible, and it is well tracked as shown by the velocity valuesafter about 100 seconds.

FIG. 23 with graphs 2310 and 2320 shows the results of controlexperiments in which two quadrotors were tracking non-colliding circulartrajectories with 180 degrees phase shift while a human (see line 2311)moved in the arena (test flying space). The human was considered as adynamic obstacle. The two quadrotors avoided collisions locally whiletracking, as closely as possible, their ideal trajectories. Graph 2310shows the trajectories, and graph 2320 shows the measured velocity forall agents. No collision or deadlock was observed in the experiment,although in some cases a quadrotor had to stop as the optimizationbecame infeasible. In this experiment, the human moved at a speedsimilar to that of the quadrotors (<2 m/s) and was slightly predictablein movements, due to a constant velocity assumption of the experiment.

At this point in the description, it may be useful to provide adiscussion of the overall behavior of the control techniques over allthe performed experiments based on statistics collected and processed bythe inventors. Deadlock was not observed in the experiments, but it ispossible from a theoretical standpoint and can occur either due to astop condition being the optimal solution with respect to theoptimization objective or due to the optimization problem beinginfeasible. The optimization can be infeasible for two main reasons: (1)for experimental reasons such as the unmodeled high-order dynamics ofthe vehicle and other uncalibrated parameters such as delays and (2) forreasons inherent in the method (and the trade-off between richness ofplanning versus low computational time) such as the limited horizon ofthe local planning or the convexification of the optimization via thelinearization of the collision avoidance constraint.

The local planning optimization, which was computed at 10 Hz, wasinfeasible in under nine percent of the of the cumulated time steps overall experiments (over 40,000 data points at 10 Hz over more than 50experimental runs). On encountering this condition, the speed of thequadrotor is reduced at a higher than normal rate following Eq. (28),and the quadrotor can, if necessary, be brought to a halt before acollision due to the finite local planning horizon. In all cases, due tothe reduction in speed, the optimization became feasible after a furtherone or more time steps, e.g., below 0.5 seconds.

FIG. 24 illustrates with graphs 2410, 2420, and 2430 optimal referencevelocity, u*, resulting from the optimization with respect to time forone of the experiments with four quadrotors. Six instances in which theoptimization is infeasible are observed between the times 65 seconds and68 seconds. These are points where the optimal reference velocitybecomes zero before the quadrotors have converged to their goalpositions. The effect of these infeasible time steps is to slow down thequadrotors, which in turn has the effect of rendering the local planningfeasible in subsequent time steps. This extreme situation was mostly notobserved in experiments with only two quadrotors.

FIG. 25 illustrates a histogram 2500 showing statistics accumulated forthe inter-agent distance over the experiments run by the inventors withquadrotor helicopters as the controlled aerial vehicles in a fly space(or 3D space). There is a step decrease in the number of data pointswith distance below one meter. This is due to the fact that thequadrotors used in the experiment were considered to have a radius of0.35 meters enlarged by ε=0.1 meters to account for the dynamics, whichcan decrease towards zero when in close proximity. Note, a few datapoints appear to be in collision (<0.7 meters), which can happen incases where the optimization becomes infeasible especially when ahuman/dynamic obstacle is moving through the test fly or 3D space. Inthe fully controlled case, no collision was visually observed and allsituations were successfully resolved. In the case with a human present,the quadrotor halted before a collision in a few cases, and thishappened, for example, when the velocity estimation is inaccurate or thehuman moves too fast or with an abrupt change in direction.

With regard to evaluation of the algorithm, good performance wasobserved throughout the experiments especially for the centralizedconvex optimization. The distributed optimization performs well insimulation and in experiments with two quadrotors. If the collisionavoidance constraints are linearized with respect to the currentvelocity, the distributed approach suffers from inaccuracies in thevelocity estimation, which can lead to performance degradation due todisagreements in the linearization. The centralized, non-convexoptimization performs well in simulation but may be too slow in somecases for real-time control/performance.

In support of the above discussion, it may be useful to provideequations or pseudocode used for determining repulsive velocity with thecontroller modules of the present shared control system. For therepulsive velocity field of FIG. 14, the repulsive velocity for agent i∈ A is given by:

for j = 1 : m, j ≠ i do  if p_(ij) ^(H) < r_(i) + r_(j) and |p_(ji) ³| <(h_(i) + h_(j) + D_(h)) then    $u_{ij}^{3} = {{V_{r}\left( \frac{r_{i} + r_{j} - p_{ij}^{H}}{r_{i} + r_{j}} \right)}\left( \frac{\left. {h_{i} + h_{j} + D_{h} -} \middle| {p_{j}^{3} - p_{i}^{3}} \right|}{D_{h}} \right)\frac{p_{ji}^{3}}{\left| p_{ij}^{3} \right|}}$ end if   if p_(ij) ^(H) < r_(i) + r_(j) + D_(r) and |p_(ji) ³| <(h_(i) + h_(j)) then    ${\overset{{^\circ}}{u}}_{ij}^{H} = {{V_{r}\left( \frac{\left. {h_{i} + h_{j} -} \middle| p_{ij}^{3} \right|}{h_{i} + h_{j}} \right)}\left( \frac{r_{i} + r_{j} + D_{r} - p_{ij}^{H}}{D_{r}} \right)\frac{p_{ji}^{H}}{p_{ji}^{H}}}$  end if  end for  ${{\overset{{^\circ}}{u}}_{i}\mspace{14mu} \text{:=}\mspace{14mu} \Sigma_{j = 1}^{m}\mspace{14mu} {{}_{}^{}{}_{}^{}}};{{\overset{{^\circ}}{u}}_{i} = {{\min \left( {\frac{V_{r}}{\parallel {\overset{{^\circ}}{u}}_{i} \parallel},1} \right)}{\overset{{^\circ}}{u}}_{i}}}$where V_(r) is the maximum repulsive force and D_(r), and D_(h) are thepreferred minimal inter-agent distances in the X-Y plane and the Zcomponent, respectively.

if p_(ij) ^(H) > r_(ij) then ∝ = atan2(p_(ij) ^(H)), ∝_(n) =acos(r_(ij)/p_(ij) ^(H)) if |p_(ij) ³| > h_(ij) then$\propto_{1}{= {{atan}\left( {\frac{p_{ij}^{3} + h_{ij}}{p_{ij}^{H} - {{{sign}\left( p_{ij}^{3} \right)}r_{ij}}} - \frac{\pi}{2}} \right)}}$$\propto_{2}{= {{atan}\left( {\frac{p_{ij}^{3} - h_{ij}}{p_{ij}^{H} + {{{sign}\left( p_{ij}^{3} \right)}r_{ij}}} + \frac{\pi}{2}} \right)}}$O = [p_(ij) ^(H) − r_(ij), p_(ij) ³ − sign(p_(ij) ³)h_(ij)] else$\propto_{1}{= {{atan}\left( {\frac{p_{ij}^{3} + {{{sign}\left( p_{ij}^{3} \right)}h_{ij}}}{p_{ij}^{H} - r_{ij}} - {{{sign}\left( p_{ij}^{3} \right)}\frac{\pi}{2}}} \right)}}$$\propto_{2}{= {{atan}\left( {\frac{p_{ij}^{3} - {{{sign}\left( p_{ij}^{3} \right)}h_{ij}}}{p_{ij}^{H} - r_{ij}} + {{{sign}\left( p_{ij}^{3} \right)}\frac{\pi}{2}}} \right)}}$O = [p_(ij) ^(H) − r_(ij), 0] end if ∝_(O) = atan2(O) n_(ij) ¹ = [cos(∝− ∝_(n)), sin(∝ − ∝_(n)), 0] n_(ij) ² = [cos(∝ + ∝_(n)), sin(∝ + ∝_(n)),0] n_(ij) ³ = [p_(ij) ^(H)cos(∝₁)/p_(ij) ^(H), sin(∝₁)] n_(ij) ⁴ =[p_(ij) ^(H)cos(∝₂)/p_(ij) ^(H), sin(∝₂)] n_(ij) ⁵ = [p_(ij)^(H)cos(∝_(O))/p_(ij) ^(H), sin(∝_(O))] c_(ij) ¹ = c_(ij) ² = c_(ij) ³ =c_(ij) ⁴ = 0; c_(ij) ⁵ = ∥O∥/τ else if |p_(ij) ³| > h_(ij) then${\propto {= {{atan}\; 2\left( p_{ij}^{H} \right)}}},{\beta = {{atan}\left( \frac{\left| p_{ij}^{3} \middle| {- h_{ij}} \right.}{r_{ij}} \right)}}$$\propto_{1}{= {\frac{\pi}{2} - {{atan}\left( \frac{\left| p_{ij}^{3} \middle| {- h_{ij}} \right.}{r_{ij} - p_{ij}^{H}} \right)}}}$$\propto_{2}{= {\frac{\pi}{2} - {{atan}\left( \frac{\left| p_{ij}^{3} \middle| {- h_{ij}} \right.}{r_{ij} + p_{ij}^{H}} \right)}}}$n_(ij) ¹= [cos ∝ cos ∝₁, sin ∝ cos ∝₁, sin ∝₁] n_(ij) ²= [cos ∝ cos ∝₂,sin ∝ cos ∝₂, sin ∝₂]$n_{ij}^{3} = \left\lbrack {{{\cos \left( {\propto {+ \frac{\pi}{2}}} \right)}\sin \; \beta},{{\sin \left( {\propto {+ \frac{\pi}{2}}} \right)}\sin \; \beta},{\cos \; \beta}} \right\rbrack$$n_{ij}^{4} = \left\lbrack {{{- {\cos \left( {\propto {+ \frac{\pi}{2}}} \right)}}\sin \; \beta},{{- {\sin \left( {\propto {+ \frac{\pi}{2}}} \right)}}\sin \; \beta},{\cos \; \beta}} \right\rbrack$n_(ij) ⁵ = [0, 0, 1] c_(ij) ¹ = c_(ij) ² = c_(ij) ³ = c_(ij) ⁴ = 0;c_(ij) ⁵ = (|p_(ij) ³| − h_(ij))/τ if p_(ij) ³ < 0 then n_(ij) ^(s)[3] =−n_(ij) ^(s)[3] ∀s  end if else Agents i and j are in collision n_(ij)^(l) = p_(ij)/p_(ij) c_(ij) ^(l) = −(r_(ij) − p_(ij) ^(H) + h_(ij) −|p_(j) ³ − p_(i) ³|/τ end ifwhere H_(ij) ¹ and H_(ij) ² represent avoidance to the right/left,H_(ij) ³ and H_(ij) ⁴ above/below and H_(ij) ⁵ represents a head-onmaneuver, which remains collision-free up to t=τ.

The reader may also benefit from being aware of the equations forlinearization of VO utilized in one implementation of the control systemby the inventors. One can denote h_(ij)=h_(i)+h_(j) andr_(ij)=r_(i)+r_(j). The non-convex constraint

³\VO_(ij) ^(T) is linearized to obtain a convex problem. For anapproximation with five linear constraints, as in FIG. 16, the linearconstraints are given by:

if p_(ij) ^(H) > r_(ij) then  ∝ = atan2(p_(ij) ^(H)), ∝_(n) =acos(r_(ij)/p_(ij) ^(H))  if |p_(ij) ³| > h_(ij) then   $\propto_{1}{= {{atan}\left( {\frac{p_{ij}^{3} + h_{ij}}{p_{ij}^{H} - {{{sign}\left( p_{ij}^{3} \right)}r_{ij}}} - \frac{\pi}{2}} \right)}}$  $\propto_{2}{= {{atan}\left( {\frac{p_{ij}^{3} - h_{ij}}{p_{ij}^{H} + {{{sign}\left( p_{ij}^{3} \right)}r_{ij}}} + \frac{\pi}{2}} \right)}}$  O = [p_(ij) ^(H) − r_(ij), p_(ij) ³ − sign(p_(ij) ³)h_(ij)]  else   $\propto_{1}{= {{atan}\left( {\frac{p_{ij}^{3} + {{{sign}\left( p_{ij}^{3} \right)}h_{ij}}}{p_{ij}^{H} - r_{ij}} - {{{sign}\left( p_{ij}^{3} \right)}\frac{\pi}{2}}} \right)}}$  $\propto_{2}{= {{atan}\left( {\frac{p_{ij}^{3} - {{{sign}\left( p_{ij}^{3} \right)}h_{ij}}}{p_{ij}^{H} - r_{ij}} + {{{sign}\left( p_{ij}^{3} \right)}\frac{\pi}{2}}} \right)}}$  O = [p_(ij) ^(H) − r_(ij), 0]  end if  ∝_(O) = atan2(O)  n_(ij) ^(l) =[cos(∝ − ∝_(n)), sin(∝ − ∝_(n)), 0]  n_(ij) ² = [cos(∝ + ∝_(n)), sin(∝ +∝_(n)), 0]  n_(ij) ³ = [p_(ij) ^(H)cos(∝₁)/p_(ij) ^(H), sin(∝₁)]  n_(ij)⁴ = [p_(ij) ^(H)cos(∝₂)/p_(ij) ^(H), sin(∝₂)]  n_(ij) ⁵ = [p_(ij)^(H)cos(∝_(O))/p_(ij) ^(H), sin(∝_(O))]  c_(ij) ^(l) = c_(ij) ² = c_(ij)³ = c_(ij) ⁴ = 0; c_(ij) ⁵ = ∥O∥/τ else if |p_(ij) ³| > h_(ij) then  ${\propto {= {{atan}\; 2\left( p_{ij}^{H} \right)}}},{\beta = {{atan}\left( \frac{\left| p_{ij}^{3} \middle| {- h_{ij}} \right.}{r_{ij}} \right)}}$ $\propto_{1}{= {\frac{\pi}{2} - {{atan}\left( \frac{\left| p_{ij}^{3} \middle| {- h_{ij}} \right.}{r_{ij} - p_{ij}^{H}} \right)}}}$ $\propto_{2}{= {\frac{\pi}{2} - {{atan}\left( \frac{\left| p_{ij}^{3} \middle| {- h_{ij}} \right.}{r_{ij} + p_{ij}^{H}} \right)}}}$ n_(ij) ¹= [cos ∝ cos ∝₁, sin ∝ cos ∝₁, sin ∝₁]  n_(ij) ²= [cos ∝ cos∝₂, sin ∝ cos ∝₂, sin ∝₂]  $n_{ij}^{3} = \left\lbrack {{{\cos \left( {\propto {+ \frac{\pi}{2}}} \right)}\sin \; \beta},{{\sin \left( {\propto {+ \frac{\pi}{2}}} \right)}\sin \; \beta},{\cos \; \beta}} \right\rbrack$ $n_{ij}^{4} = \left\lbrack {{{- {\cos \left( {\propto {+ \frac{\pi}{2}}} \right)}}\sin \; \beta},{{- {\sin \left( {\propto {+ \frac{\pi}{2}}} \right)}}\sin \; \beta},{\cos \; \beta}} \right\rbrack$ n_(ij) ⁵ = [0, 0, 1]  c_(ij) ¹ = c_(ij) ² = c_(ij) ³ = c_(ij) ⁴ = 0;c_(ij) ⁵ = (|p_(ij) ³| − h_(ij))/τ  if p_(ij) ³ < 0 then   n_(ij)^(s)[3] = −n_(ij) ^(s)[3] ∀s  end if else  Agents i and j are incollision  n_(ij) ^(l) = p_(ij)/p_(ij)  c_(ij) ^(l) = −(r_(ij) − p_(ij)^(H) + h_(ij) − |p_(j) ³ − p_(i) ³|/τ end ifwhere H_(ij) ¹ and H_(ij) ² represent avoidance to the right/left,H_(ij) ³ and H_(ij) ⁴ represent avoidance above/below, and H_(ij) ⁵represents a head-on maneuver, which remains collision-free up to t=τ.

Control systems, such as system 100 of FIG. 1, and shared controlmethods, such as the method 300 of FIG. 3, can be used and/or expandedto provide cooperative collision avoidance among decision-making agents(or vehicles such as wheeled and/or other terrestrial or ground-basedvehicles). These control systems and methods may apply to collision-freenavigation among heterogeneous groups of decision-making agents. Thefollowing paragraphs and discussion highlights several contributionsprovided by the inventors including: (1) arbitrary kinematic model anddynamic constraints; (2) convex and non-convex, centralized anddistributed algorithms for collision avoidance; (3) a novel extension totruly cooperative distributed collision avoidance where a predictionover future velocities of the neighboring agents can be taken intoaccount in the computation of the velocity partition; and (4) extensiveexperimental evaluation of the proposed algorithms including differentrobot kinematics (e.g., small differentially driven robots, car-likerobots, robotic wheelchairs, and boats). Although the followingdescription generally focuses on vehicles/agents navigating in 2D space,the control methods can be used for aerial vehicles navigating in 3Dspace.

Throughout the following paragraphs, x denotes scalars, x vectors, Mmatrices, and X sets. The Minkowsky sum of two sets is denoted by X⊕yand x=∥x∥ denotes the Euclidean norm of vector x. The super index *^(k)indicates the value at time t^(k) and {tilde over (t)}=t−t^(k) theappropriate relative time. Subindex *_(i) indicates agent i, andrelative vectors are denoted by x_(ij)=x_(i)−x_(j). For ease ofexposition, no distinction is made between estimated and real states.

The control problem can be stated as a group of n agents freely movingin 2D space for which shared control is required or desired. For eachagent i, p_(i) ∈

³ denotes its position, v_(i)={dot over (p)}_(i) its velocity, anda_(i)={umlaut over (p)}_(i) its acceleration. The shape of each agent ismodeled by an enveloping circle of radius r_(i), centered at the agent'sposition p_(i), and denoted by D(p_(i),r_(i)). One goal of the controlmethod is to compute for each agent, at high frequency (e.g., 10 Hz), alocal motion that respects the kinematics and dynamic constraints of theego agent and that is collision free with respect to neighboring agentsfor a short time horizon (typically, 2 to 5 seconds or the like), andthe problem may be labeled simply as “collision avoidance.” As discussedabove, centralized and distributed scenarios can be considered andaddressed with local motion planning using the same terminology (e.g.,global trajectory, preferred velocity, local trajectory, candidate localtrajectory, candidate trajectory, and optimal reference trajectory) asdiscussed with reference to FIGS. 13A and 13B.

With regard to definition of candidate local trajectories, eachcandidate is that of an omnidirectional agent moving at a constantvelocity, u, and starting at the current location, p^(k), of the agent:

p _(ref)(t)=p ^(k) +u(t−t ^(k)), t ∈ [t ^(k),∞)   Eq. (41)

A trajectory tracking controller, respecting the kinematics and dynamicconstraints of the agent, p({tilde over (t)})=f(z^(k),u,{tilde over(t)}) is considered that is continuous in the initial statez^(k)=[p^(k),{dot over (p)}^(k),{umlaut over (p)}^(k), . . . ] of theagent and converging to the candidate reference trajectory. This definesthe candidate local trajectories given by p({tilde over (t)}) and abijective mapping from candidate reference velocities u.

One focus of the inventors' work is on local motion planning using apreferred velocity, ū, computed by a guidance system in order to track aglobal trajectory. An optimal reference trajectory can be computed suchthat its associated local trajectory is collision free. This provides aparameterization of local motions given by u and allows for an efficientoptimization in candidate reference velocity space, (

³), to achieve collision-free motions. FIG. 26 illustrates withfunctional block or data flow diagrams 2610 and 2620 local motionplanning according to the present teaching that uses a distributedapproach and using a centralized approach, respectively.

Velocity prediction involves consideration that in each time instance aprobability distribution over the future velocities of the ego agent andits neighbors is available or, due to the structure of the problem, itcan be relaxed to a utility over the possible future velocities thatrepresents their individual likelihood. This distribution can becomputed in an environment around the agent's current velocity. Given acandidate reference velocity, ui, for an agent, i, its utility isdefined by a probability distribution:

Utility, φ_(i):

²→[0,1] u_(i)

φ_(i)(u_(i))   Eq. (42)

such that ∫_(u) _(i) _(∈)

₂ φ_(i)(u_(i))=1. A value close to 1 indicates that the agent is verylikely to choose that reference velocity, while a value close to 0indicates that it is very unlikely.

In multi-agent applications where several robots/agents/vehicles arecontrolled, a prediction over the future velocities of the neighboringagents may not be suitable. In that case, a cost can be defined:

Cost, χ_(i):

²→[0,∞) u_(i)

χ_(i)(u_(i))   Eq. (43)

which is then transformed into a utility distribution with a mapping[0,∞)→[0,1]. To maintain the linear characteristics for low cost, onecan employ the following (but, note, other mappings are possible and maybe used):

φ_(i,aux)(u _(i))=max(0,1−χ_(i)(u _(i))/K ₀)   Eq. (44)

where K₀ is the cut-off cost, and this can be normalized to

$\begin{matrix}{{\varphi_{i}\left( u_{i} \right)} = \frac{\varphi_{i,{aux}}\left( u_{i} \right)}{\int_{u_{i} \in R^{2}}{\varphi_{i,{aux}}\left( u_{i} \right)}}} & {{Eq}.\mspace{11mu} (45)}\end{matrix}$

In the following paragraphs, three cases are discussed: (1) a simplifiedprediction; (2) utility computed from a motion prediction; and (3)utility computed from a cost distribution.

Using a simplified prediction may be useful in some implementations of acontroller or control system. Simplifying assumptions can be made withrespect to the neighboring agents such as the following: (1) staticagent to provide a zero-order approximation (φ_(j)(u_(j))=1, foru_(j)=0; φ_(j)(u_(j))=0 otherwise); (2) constant velocity for afirst-order approximation (φ_(j)(u_(j))=1, for u_(j)=v_(j);φ_(j)(u_(j))=0); and (3) constant preferred velocity to provide afirst-order approximation (φ_(j)(u_(j))=1, for u_(j)=ū_(j);φ_(j)(u_(j))=0). In these formulations of the velocity predictions,ū_(j) is the preferred velocity of agent j.

In other cases, the velocity prediction may be provided using a utilitygiven a motion prediction. As a step towards accounting foruncertainties and predictions, one may consider the case of having aprediction of the future motion of the neighboring agent, i, given by aGaussian Process (GP) over its future positions and velocities. Theutility of each reference velocity can then be obtained by computing theposterior probability given the Gaussian Process, with different levelsof complexity and using the following: (1) reference velocity(φ_(j)(u_(j))=P(u_(j),GP)); (2) candidate reference trajectory(φ_(j)(u_(j))=P(p_(k) ^(k)+u_(j)(t−t^(k)),GP(t))); and (3) candidatelocal trajectory (φ_(j)(u_(j))=P(f(z_(j) ^(k),u_(j),t),GP(t))).

In still other cases, the velocity prediction may be provided using autility given by a cost from the environment. The environmentcharacteristics can be taken into account when constructing the weightsover the reference velocities, u_(j). This approach then providesinformation about the maneuverability of the neighboring agents andtheir motion preferences and can later be incorporated in the localmotion planning step.

As an example, the cost, χ_(j)(u_(j)), can be given by the sum ofseveral terms, which may include the following and should be saturated:(1) deviation from current velocity, where the agent is expected tocontinue with its current velocity (K₁∥u_(j)−v_(j)∥²); (2) deviationfrom preferred velocity, where velocities close to its preferredvelocity incur lower cost (K₂∥u_(j)−ū_(j)∥²); (3) cost to go, determinedas being proportional to the distance from the desired goal to theexpected position after the time horizon of the local planner and as maybe determined with a search algorithm or from a pre-computed distancetransform map, and, with K₃ being a design constant, the term can benormalized to [0,K₃v_(max)τ] by K₅max (0, cost2go(p_(j) ^(k)+u_(j)τ)−C₀)Eq. (46) where C₀=max (0, cost2go(p_(j) ^(k))−v_(max)τ); (4) staticobstacles, where a high cost, K₄→∞, is added for reference trajectoriesin collision with a static obstacle such that p_(j) ^(k)+u_(j){tildeover (t)} ∈ O_(r) for some {tilde over (t)} ∈ [0,τ]; (5) congestion,where a cost, K₅<<K₄, can be added for reference trajectories incollision with another agent for {tilde over (t)} ∈ [0,τ]; for thisweight, the other agents can be considered static or moving at constantspeed, and the intuition behind this is that an agent would prefer tomove away from other neighbors but, since they are mobile, the cost of apotential collision is not too high; note, this term penalizes movementtowards potentially congested areas and may only be used for guidance ashard collision avoidance constraints are included in the local motionplanning framework; and (6) motion velocity field, where the case isconsidered where a motion velocity field of the environment is availablegiven by a velocity, V(p), for each position p, and this is the case,for example, in multi-lane navigation, with intuition indicating thatvelocities against the flow field are unlikely; this could be encoded byK₅∫₀ ^(τ)∥V(p_(j) ^(k)+u_(j)t)−u_(j)∥dt for areas where the velocityfield is defined and 0 in free areas (but alternative formulations arepossible).

The different cost teams represent the belief the agent has over theenvironment and their relative weights are design parameters. Costs 1and 4 to 6 can be obtained by all agents with common observations. Onthe other hand, costs 2 and 3 are agent specific and cannot be observed,although an estimation may be available and used. These terms are givenas an example and can be modified or simplified. One contribution ofthis description is that of incorporating the velocity prediction in theplanning.

Now, turning to determination of preferred velocity, in each time step,each agent, computes a preferred velocity, ū_(i), which would be itsdesired reference velocity in the absence of other agents. It can beobtained to achieve different objectives. A first objective may be goaldirected. In this case, the preferred velocity, ū_(i), can be computedfrom the “cost to the goal” map (e.g., obtained by an F1 expansion asdiscussed above). Its orientation is that of the shortest path to thegoal from the current position of the agent, and its magnitude equalsthe preferred speed, V>0, of the agent reduced linearly in theneighborhood of the goal.

A second objective may be trajectory following. In this case, thepreferred velocity, ū_(i), is given by a trajectory tracking controllerfor omnidirectional agents, such as a controller in position with feedforward velocity, such as:

ū=K _(p)(q _(i) −p _(i))+{dot over (q)} _(i)   Eq. (47)

where q_(i) is the ideal position on the guidance trajectory at thecurrent time and K_(p) is a design constant. A third objective may besemi-autonomous driving. In this case, the preferred velocity, ū_(i),can be specified by a driver, with the preferred velocity, ū_(i), beingproportional to the driving wheel or joystick position (in the relativeframe of the vehicle).

In practice, the preferred velocity, ū_(i), often will be corrected witha repulsive velocity,

_(i), that pushes the vehicle away from both static and dynamicobstacles. Collision-free motion is guaranteed by the constraints thatare directly included in the local planning framework. Optimal solutionsthat minimize deviation with respect to the preferred velocity doproduce solutions where agents come arbitrarily close to each other.This behavior can be unsafe in some real world scenarios withuncertainties in sensing and kinematic model. The repulsive velocity is,thus, included to help maintain a minimum distance to other agents andwalls, and it moves the ego-agent (controlled vehicle) away when it istoo close.

A linear function weighting the distance to other vehicles can be usedthat is adapted to specify the maximum velocity allowed towards theobstacles or other vehicles as shown by:

$\begin{matrix}{{\overset{.}{u}}_{i} = {{u_{i{o}}^{rep}n_{i,o}} + {\sum_{{{j}p_{ij}} < D}{u_{i{j}}^{rep}\frac{p_{ij}}{p_{ij}}}}}} & {{Eq}.\mspace{11mu} (48)} \\{where} & \; \\{u_{i{o}}^{rep} = {\max \left( {{{V_{r}\frac{\overset{\_}{D} - {d\left( {p_{i},O_{\tau_{i}}} \right)}}{\overset{\_}{D}}} - \overset{\_}{V}},{{\overset{\_}{u}}_{i} \cdot n_{i,o}}} \right)}} & {{Eq}.\mspace{11mu} (49)} \\{u_{i{j}}^{rep} = {\max \left( {{{V_{r}\frac{D - p_{ij}}{D - r_{i} - r_{j}}} - \overset{\_}{V}},{{\overset{\_}{u}}_{i} \cdot \frac{p_{ij}}{p_{ij}}}} \right)}} & {{Eq}.\mspace{11mu} (50)}\end{matrix}$

where V_(r) is the maximum repulsive velocity, D and D are the maximumdistance from which a repulsive force is added, d(p_(i),O) is thedistance to the closest static obstacle within the O_(r) _(i) map andn_(i,O) is the normal vector from it. In order to avoid corner issues,not only the closest point it considered but also an average over theclosest points. The norm of

_(i) is then limited to ∥

_(i)∥≦V_(r). For ease of notation, consider the following ū:=ū+

.

Turning now to consideration of motion constraints, for an agent, i, thecandidate local trajectories can be given by a tracking controller,p_(i)(t)=f(z_(i) ^(k),u_(i),{tilde over (t)}), towards a constantvelocity reference trajectory parameterized by u. The set of feasiblelocal trajectories given the current state is defined by:

_(i)(z _(i) ^(k))={u _(i) ∈

², such that the trajectory f(z _(i) ^(k) ,u _(i) ,{tilde over (t)})respects all dynamic constraints}  Eq. (51)

And, in the following discussion, the super-index, *^(k), is dropped tosimplify the notation.

With regard to a first constraint regarding dynamic restrictions(“Constraint 1”), in order to guarantee collision-free motions, itshould be guaranteed that each agent remains within a maximum trackingerror, ε_(i), of its reference trajectory. For a maximum tracking errorε_(i) and a current state, z_(i)=[p_(i),{dot over (p)}_(i),{umlaut over(p)}_(i)], the set of reference velocities u_(i) that can be achievedwith position error below ε_(i) is given by R_(i):=R(z_(i),ε_(i)) andthen:

R _(i) :={u _(i) ∈

_(i)(z _(i))|∥(p _(i) +{tilde over (t)}u _(i))−f(z _(i) ,u _(i) ,{tildeover (t)})∥≦ε_(i) ,∀{tilde over (t)}>0}  Eq. (52)

If a local trajectory that is continuous in acceleration is used (suchas one generated by one of the trajectory controllers taught herein),the set of reachable reference velocities, R_(i), depends on {dot over(p)}_(i), {umlaut over (p)}_(i), and ε_(i). A mapping, γ, from theinitial state, {dot over (p)}_(i), {umlaut over (p)}_(i), and areference velocity, u_(i), to maximum tracking error is precomputed andstored in a look up table as:

$\begin{matrix}{{\gamma \left( {u_{i},{\overset{.}{p}}_{i},{\overset{¨}{p}}_{i}} \right)} = {\begin{matrix}\max \\{\overset{\sim}{t} > 0}\end{matrix}{{{\left( {p_{i} + {\overset{\sim}{t}u_{i}}} \right) - {f\left( {z_{i},u_{i},\overset{\sim}{t}} \right)}}}}}} & {{Eq}.\mspace{11mu} (53)}\end{matrix}$

A bounding box given by four linear constraints of the formH_(l)={u_(i)|n_(l)u_(i)≦b_(l)} is computed such that R_(i) ⊂ ∪_(l=1)⁴H_(l). These linear constraints can then be included in the algorithmsto reduce the search space.

Although some examples of agents, vehicles, or robots have been providedearlier, it may be useful at this point in the description to provideseveral additional examples of typical robot models used by theinventors to implement and/or test their control methods (andcontrollers and control systems). Below, five different robot kinematicsand dynamics models are described that have been implemented and testedwith this framework, and these cover most of the typical robot platformspresently in use.

First, one may model a robot, vehicle, or agent as a simply unicyclethat may take the form of a small, differently-drive robot. In thiscase, the local trajectories can be defined by two sections including acircumference arc followed by a straight line path at a constant speed,u_(i). The angular velocity over the arc was, in one model, defined suchthat the correct orientation was achieved within a fixed time, T, andthe linear velocity minimized the tracking error of the referencetrajectory. A closed form solution was obtained for the set R_(i), but,in this simple formulation, the robots had no constraints inacceleration and the linear and angular velocities presented adiscontinuity.

Second, one may model the controlled agent as a bicycle. For example,this modeling may be applied to car-like robots with bicycle kinematics.A trajectory controller can be employed to track the referencedtrajectory given by a constant speed, u_(i), and was obtained byapplying full-state linearization via dynamic feedback to the non-linearsystem. Additional constraints such as maximum steering angle andvelocity and linear velocity and acceleration were added as saturationlimits. This framework was useful as it guaranteed continuity in bothlinear velocity and steering angle.

Third, a point mass v^(th)-order integrator may be used to model thecontrolled vehicle. A controller for point mass v^(th)-order integratordynamics can and has been described, which is continuous up to thev_(th) derivative of the initial state. For v>0, the state feedbackcontrol law was given by:

p ^((v+1))({tilde over (t)})=−a _(v) p ^((v)) − . . . −a ₂ {umlaut over(p)}+a ₁(u−{dot over (p)})+a ₀(p ^(k) +u{tilde over (t)}−p)   Eq. (54)

In particular, for v=2 (continuity in initial acceleration), thetrajectory is given by:

p({tilde over (t)})=p ^(k) +u{tilde over (t)}+(({dot over (p)} ^(k)−u+(ω₁−ω₀)F){tilde over (t)}−F)e ^(−ω) ⁰ ^({tilde over (t)})  Eq. (55)

with F=({umlaut over (p)}^(k)+2ω₀({dot over (p)}^(k)−v∞))/(ω₀−ω₁)² andω₀, ω₁ being design parameters related to the decay of the initialvelocity and acceleration.

Fourth, the robot may be modeled as a unicycle with dynamic constraints.For example, as shown in FIG. 2, a robotic wheelchair may be modeled asa unicycle. The kinematic model of a differentially-driven agent(unicycle) was considered:

[ p,{dot over (δ)}] ^(T)=[cos δ, sin δ, 0]^(T) v ₁+[0, 0, 1]^(T) v ₂  Eq. (56)

with p being the point in between both tractor wheels, δ being theorientation of the vehicle, and v₁, v₂ being the forward and rotationalspeed of the platform, respectively. The relevant variables are shown inFIG. 2. For this non-linear system, p is not fully controllable. Anyreference point, p, to the front of the vehicle axles is fullycontrollable and linearization via static feedback becomes feasible.Consider p= p+[cos δ, sin δ]^(T)Δ, where Δ>0 defines the maneuverabilityof the vehicle. The following relations then hold:

{dot over (p)}=ξ, v ₁=ξ·[cos δ, sin δ]^(T) , v ₂=[−sin δ, cosδ]^(T)/Δ  Eq. (57)

and the system is then controllable about p with holonomic velocityinputs given by ξ ∈

² and a local trajectory may be applied at this point. A circle with aradius, r, is considered as the enveloping shape of the vehicle.Equation (55) defines the local motion of the vehicle's holonomic point.Additional constraints can be added such as maximum linear and angularvelocity (|v₁|≦v_(1,max), |v₂|≦v_(2,max)) and maximum linear and angularaccelerations (|{dot over (v)}₁|≦{dot over (v)}_(1,max), |{dot over(v)}₂|≦{dot over (v)}_(2,max)).

Fifth, the vehicle may be modeled as an omnidirectional boat withdynamic constraints. The methods taught herein have also been applied tocontrol a team of omnidirectional boats controlled by three rotatingpropellers. An LQR controller in position in velocity was employed totrack the reference trajectory. Additional saturation limits wereincluded that accurately model the boat dynamics.

In the control method, static obstacles are typically given by a gridmap, O₀, of occupied space. The dilated obstacle map of the positions,p, where a disk, D(p,r), is in collision with a static obstacle isdenoted by O_(r). Its complement defines the positions for which theagent is not in collision. A constraint (“Constraint 2” in some of thisdescription) can be stated regarding collision avoidance with staticobstacles. For agent i≦m this constraint is given by the referencevelocities such that the new positions are not in collision with theenlarged obstacle map, p_(i)+u_(i){tilde over (t)} ∉ O_(r) _(i) _(+ε)_(i) , for all {tilde over (t)} ∈ [0,τ].

The constraint can be kept in its original grid-based representation orconvexified. Grid-based forms can be used due to the mapping betweenvelocities and future positions. This provides a vehicle with fullnavigation capability including being able to move close to the staticobstacles and being able to go inside small openings. Particularly,given a known grid map, O₀, the enlarged map O_(r) _(i) _(+ε) _(i) canbe precomputed for several values of ε_(i). With regard toconvexification, for simplicity and lower computational cost, the staticobstacles within a neighborhood of the robot may be approximated by aunion of linear constraints forming a convex polygon. This is the case,of example, for a polygon where the closest side is taken as the linearconstraint, but, in complex environments, this can be suboptimal.Depending on the algorithm employed either the grid-based representationor its convex approximation can be employed.

With regard to pair-wise collision avoidance, the following discussionteaches the constraint for inter-agent avoidance (or avoidance ofdynamic obstacles), and it may be based on work on velocity obstacles,reciprocal velocity obstacles, and/or their convex counterpart. Theconstraint is computed for agents of radius {circumflex over(r)}_(i)=r_(i)+ε_(i) following the reference trajectory. First, theconstraint is described in its relative velocity space formulation,which can be employed in a centralized optimization. Then, threedifferent approaches to obtain a distributed version are described.

Regarding a velocity obstacle and relative velocity space, theinter-agent collision avoidance constraint can be obtained for thereference trajectory following or using work on velocity obstacles. Thevelocity obstacle is formed by the relative reference velocities thatlead to a collision between the two agents within the given time horizonτ. This constraint (“Constraint 3” or a constraint to provideinter-agent collision avoidance) may be stated: for every pair ofneighboring agents i≦m, j ∈ [i+1,n], the constraint is given be thecandidate reference velocities such that∥p_(i)−p_(j)+(u_(i)−u_(j)){tilde over (t)}∥≧{circumflex over(r)}_(i+j)={circumflex over (r)}_(i)+{circumflex over (r)}_(j), for all{tilde over (t)} ∈ [0,τ]. This constraint can be rewritten asu_(i)−u_(j) ∉ VO_(ij) ^(τ)=∪_({tilde over (t)}=0)^(τ)((D(p_(j),{circumflex over (r)})) ⊕ D(p_(i),{circumflex over(r)}_(i))/{tilde over (t)}), representing a truncated cone, which isonly computed if the distance between the two agents is below athreshold (p_(ij)<K_(d)). Since this constraint is non-convex (freespace is the complement of VO_(ij) ^(τ)), it should be linearized toincorporate it into a convex optimization. Alternatively, forhomogeneous teams of robots, the control obstacle (or C^(N)) can beemployed as a substitute of the velocity obstacle. Then, C^(N)-COconsiders all interacting agents or robots as linear systems with orderN dynamics. After linearization of the C^(N)-CO, the method describedherein proceeds analogously.

An approximation can be made using linear constraints. Particularly, thenon-convex constraint

²\VO_(ij) ^(τ) (inter-agent collision-free reference velocities) can beapproximated by three linear constraints of the form n_(ij)^(l)·u_(ij)−b_(ij) ^(l), with l ∈ {1,2,3} and given by:

$\begin{matrix}{{{\begin{bmatrix}{\cos \left( \gamma^{+} \right)} \\{\sin \left( \gamma^{+} \right)}\end{bmatrix}u_{ij}} \leq 0},{{{- \frac{p_{ij}}{p_{ij}}} \cdot u_{ij}} \leq \frac{p_{ij} - r_{i + j}}{\tau}},{{\begin{bmatrix}{\cos \left( \gamma^{-} \right)} \\{\sin\left( \gamma^{-} \right.}\end{bmatrix}u_{ij}} \leq 0}} & {{Eq}.\mspace{11mu} (58)}\end{matrix}$

where γ⁺=α+β, γ⁻=α−β, α=a tan 2(−p_(ij)) and β=a cos(r_(i+j)/p_(ij)).The first and last constraints represent avoidance to the right and tothe left, respectively, and the middle constraint represents a head-onmaneuver, which remains collision free up to {tilde over (t)}=τ.

Turning now to the selection of a linear constraint,

²\VO_(ij) ^(τ) is linearized by selecting one of the three linearconstraints. Sensible choices include the following: (1) fixed side foravoidance: if agents are moving towards each other (v_(ij)·p_(ij)<0)avoid on a predefined side, on the left (l=1) or on the right (l=3) andif the agents are not moving towards each other the constraintperpendicular to the apex of the cone (l=2) is selected to maximizemaneuverability; (2) maximum constraint satisfaction for the currentrelative velocity,

${\begin{matrix}{\arg \; \min} \\l\end{matrix}\left( {{n_{ij}^{l} \cdot \left( {v_{i} - v_{j}} \right)} - b_{ij}^{l}} \right)}:$

this selection maximizes the feasible area of the optimization whentaking into account the motion continuity constraints; and (3) maximumconstrain satisfaction for the preferred velocity,

$\begin{matrix}{\arg \; \min} \\l\end{matrix}\left( {{n_{ij}^{l} \cdot \left( {{\overset{\_}{u}}_{i} - {\overset{\_}{u}}_{j}} \right)} - b_{ij}^{l}} \right)$

if centralized and

$\begin{matrix}{\arg \; \min} \\l\end{matrix}\left( {{n_{ij}^{l} \cdot \left( {{\overset{\_}{u}}_{i} - v_{j}} \right)} - b_{ij}^{l}} \right)$

if distributed: this selection may provide faster progress towards thegoal position but the optimization may become quickly infeasible if theagent greatly deviates from its preferred trajectory. Option 1 providesthe best coordination results as it incorporates a social rule, Option 2maximizes the feasible area of the optimization, and Option 3 provides(or may provide) faster convergence to the ideal trajectory. Withoutloss of generality, consider n_(ij)·u_(ij)≦b_(ij) the selectedlinearization of

²\VO_(ij) ^(τ), which can be included in a centralized convexoptimization.

Pair-wise collision avoidance may also include avoidance of a velocityobstacle in the distributed case. The new reference velocity of theneighboring agent, u_(j), is unknown. One of the following assumptionscan be made regarding this velocity: (1) static agent: the assumptionu_(j)=0 is made, and the constraint is then given by u_(i) ∈

²\VO_(ij) ^(τ) and its linearization by n_(ij)·u_(i)≦b_(ij), with thisapproach being prone to collisions in highly dynamic environments; and(2) constant velocity: the assumption u_(j)=v_(j) is made, and theconstraint is then given by u_(i) ∈

²\VO_(ij) ^(τ) ⊕ v_(j) and its linearization byn_(ij)·u_(i)≦b_(ij)+n_(ij)·v_(j), with this approach typically notconsidering the case where the other agent is also decision-making.

Pair-wise collision avoidance may further include velocity obstacles (ora distributed reciprocal). In the following discussion, the assumptionis made that all agents compute collision-free motions followingidentical algorithms. One attempt towards taking into account decisionmaking can be considered the work on the reciprocal velocity obstacle,where the velocity obstacle was slightly translated and the assumptionwas made that both agents share the avoidance effort with a knownweight. To avoid reciprocal dances, to obtain stronger guarantees, andto coin the problem as a convex optimization, the idea was laterextended to optimal reciprocal collision avoidance (ORCA).

The goal with regard to the velocity obstacle is to obtain two setsF_(i|j) ⊂

² and F_(j|i) ⊂

² such that for every velocity u_(i) ∈ F_(i|j) and for every velocityu_(j) ∈ F_(j|i) the relative velocity is collision free, thus:

$\begin{matrix}\left. \left. \begin{matrix}{u_{i} \in \mathcal{F}_{i{j}}} \\{u_{j} \in \mathcal{F}_{j{i}}}\end{matrix} \right\}\Rightarrow{{u_{i} - u_{j}} \in {{\mathbb{R}}^{2}\text{\textbackslash}{VO}_{ij}^{\tau}}} \right. & {{Eq}.\mspace{11mu} (59)}\end{matrix}$

equivalent to F_(i|j) ⊕ (−F_(j|i)) ⊂

²\VO_(ij) ^(τ). Therefore, agent i can freely select a velocity withinF_(i|j) and agent j can freely select a velocity within F_(j|i).

An avoidance topology can be fixed by first linearizing the constraint

²\VO_(ij) ^(τ). In this case, the partition is reduced to obtaining avalue b_(i|j) such that:

$\begin{matrix}\left. \left. \begin{matrix}{{n_{ij} \cdot u_{i}} \leq b_{i{j}}} \\{{{- n_{ij}} \cdot u_{j}} \leq {b_{ij} - b_{i{j}}}}\end{matrix} \right\}\Rightarrow{{n_{ij} \cdot u_{ij}} \leq b_{ij}} \right. & {{Eq}.\mspace{11mu} (60)}\end{matrix}$

where the implication holds for any b_(i|j) ∈

. For reciprocal collision avoidance, b_(i|j) can be defined such thatif the minimum required change in relative velocity is denoted byΔu_(ij) then the minimum required change in velocity for agent i isΔu_(i)=λ_(i|j)Δu_(ij), with λ_(i|j) being a constant to indicate how theavoidance effort is shared between both agents. For agent j, it is thenassumed Δu_(j)=−(1−λ_(i|j))Δu_(ij). The implication of Equation (60)then holds, and, for agent i, the linear constraint is computed from:

$\begin{matrix}{{n_{ij} \cdot u_{ij}} = {{n_{ij} \cdot \left( {v_{i} - v_{j} + {\Delta \; u_{ij}}} \right)} = {{n_{ij} \cdot \left( {v_{i} - v_{j} + \frac{\Delta \; u_{i}}{\lambda_{i{j}}}} \right)} = {{n_{ij} \cdot \left( {v_{i} - v_{j} + {\left( {u_{i} - v_{i}} \right)/\lambda_{i{j}}}} \right)} \leq b_{ij}}}}} & {{Eq}.\mspace{11mu} (61)}\end{matrix}$

which implies:

n _(ij) ·u _(i) ≦b _(i|j)=λ_(i|j) b _(ij) +n _(ij)·((1−λ_(i|j))v_(i)+λ_(i|j) v _(j))   Eq. (62)

Although decision making is encoded, the parameter λ_(i|j) defining thecollaboration effort is fixed and known, typically considering a knownpartition between agents that equally cooperate, λ_(i|j)=0.5, anddynamic obstacles, λ_(i|j)=1. FIG. 27 illustrates with the functionalblock system or data flow diagram 2700 exemplary computation ofcollision constraints.

At this point in the description, it may be useful to provide moreexplanation of cooperative pair-wise collision avoidance. Reciprocalmethods are a first step towards modeling inter-agent decision making,but they consider all agents equal or a known cooperation effort.Velocity predictions and external factors such as static obstacles werenot taken into account. In contrast, the following provides a method toincorporate a velocity prediction or utility function over futurevelocities in the selection of the sets F_(i|j) and F_(j|i) of thevelocity partition for distributed collision avoidance. This produces atruly cooperative partition that takes into account the state of eachagent and can improve vehicle navigation. A cooperation measure isdefined as a function of the utility functions φ_(i)(u_(i)) andφ_(j)(u_(j)) of both interacting agents and the partition is found suchthat the cooperation measure is maximized.

With regard to the cooperation measure, to bound the problem, one canconsider two disks, D_(i) and D_(j), that are centered at the currentvelocity of each agent and of known radius, such as τa_(i) ^(max)representing the velocities that can be achieved within the timehorizon. For each linearization option, l ∈ {1,2,3}, of thecollision-free relative velocities

²\VO_(ij) ^(τ) and for each value of b_(i|j) defining the partition, thecooperative measure Y(l,b_(i|j)) is given by: (1) Y(l) which can favor acertain avoidance topology, for example to encode preferences foravoidance to the right; and (2) Y(i,l,b_(i|j)) which is a function ofthe utility of the collision-free velocities within the partitionF_(i|j) of agent i (analogously for agent j). One can then define eachagent's term by:

$\begin{matrix}{{Y\left( {i,l,b_{i{j}}} \right)} = \frac{\int_{u_{i} \in {D_{i}\bigcap\mathcal{F}_{i{j}}}}{{\varphi_{i}\left( u_{i} \right)}{u_{i}}}}{\int_{u_{i} \in D_{i}}{{\varphi_{i}\left( u_{i} \right)}{u_{i}}}}} & {{Eq}.\mspace{11mu} (63)}\end{matrix}$

with F_(i|j) and F_(j|i) defined by Equation (60). Then, two alternativecooperation measures are proposed in the form of a joint value and amin-max value. The joint value is given by:

Y ₁(l,b _(i|j))=Y(l)+Y(i,l,b _(i|j))+(j,l,b _(i|j))   Eq. (64)

The min-max value is given by:

Y ₂(l,b _(i|j))=Y(l)+min(Y(i,l,b _(i|j)),Y(j,l,b _(i|j)))   Eq. (65)

Note that, for each agent and fixed l, Y(i,l,b_(i|j)) is a monotonicallyincreasing/decreasing function of b_(i|j) taking values from 0 to 1.

With regard to an optimal partition and linearization, for a cooperativemeasure Y_(r)(l,b_(i|j)), the optimal partition and linearization arethen given by:

$\begin{matrix}{\left( {l^{*},b_{i{j}}^{*}} \right) = {\arg_{l,b_{i{j}}}^{\max}{\mathrm{\Upsilon}\left( {l,b_{i{j}}} \right)}}} & {{Eq}.\mspace{11mu} (66)}\end{matrix}$

obtained by first computing the value Y_(r)(l,b_(i|j)) for each l ∈{1,2,3} and then selecting the maximum among the three. The reciprocalpartition discussed earlier is obtained, for λ=0.5, by employing thecooperation measure:

Y _(r)(l,b _(i|j))=max(n _(ij) ^(l) ·v _(i) −b _(i|j) −n _(ij) ^(l) ·v_(i)−(b _(ij) −b _(i|j)))   Eq. (67)

The following discussion teaches another exemplary algorithm or methodfor performing local motion planning (e.g., as may be implemented withthe shared control system 100 of FIG. 1). In each control loop, anoptimal reference velocity, u*_(i), is computed by solving anoptimization in candidate reference velocity space. The preferredvelocity, ū_(i), of the ego-agent is given, together with the currentposition and velocity of neighboring agents. The following describes adistributed optimization and a centralized optimization. Theoptimization cost, which is described in detail below, is denoted byC(u_(i)) and C(u_(1:m)).

For a first optimization technique (“Optimization 1” or distributedoptimization), each agent, i, independently solves an optimization whereits optimal reference velocity, u*_(i), is computed as follows, with theposition p_(j) and velocity v_(j) of neighboring agents being known:

u_(i)^(*) = arg_(u_(i))^(min)C(u_(i)) s.t.  u_(i) ≤ u_(max)u_(i) ∈ R_(i)  respects  motion  constraintsu_(i)  collision-free  w.r.t.  static  obstacles u_(i) ∈ ℱ_(i|j)  collision-free  w.r.t.   neighboring   agents

For a second optimization technique or algorithm (“Optimization 2” orcentralized optimization), a single optimization is solved where theoptimal reference velocities of all vehicles, u*_(1:m)=[u*₁, . . . ,u*_(m)], are jointly computed as follows:

$u_{1:m}^{*} = {\arg \begin{matrix}\min \\u_{1:m}\end{matrix}{C\left( u_{i:m} \right)}}$s.t.  u_(i) ≤ u_(max), ∀i ≤ mu_(i) ∈ R_(i)  respects  motion  constraints  ∀i ≤ mu_(i)  collision-free  w.r.t  static  obstacles  ∀i ≤ mu_(i)  collision-free  w.r.t  neighboring  agents  ∀i ≤ m

Note that in the centralized optimization the partitions, F_(i|j), fordistributed avoidance are not required or utilized.

In the following, four algorithms are described to compute the optimalreference velocities. First, a distributed, convex optimization may beused. Optimization 1 is solved with quadratic cost C(u_(i)) andlinearized constraints. Computational complexity is very low andscalability is very good, but the solution space is greatly reduced dueto the linearization of the constraints and the partition of thereference velocity space. Second, a centralized, convex optimization maybe used to compute the optimal reference velocities. In this case,Optimization 2 is solved with quadratic cost, C(u_(i:m)), and linearizedconstraints. Computation complexity is low and scalability is relativelygood, but the solution space is partially reduced due to thelinearization of the constraints. Third, a centralized, non-convexoptimization may be utilized. With this algorithm, Optimization 2 issolved with quadratic cost, C(u_(i:m)), and non-convex constraints.Computational complexity is high and scalability is poor, but thecomplete solution space is explored. Fourth, a distributed, non-convexsearch may be performed within the convex region to compute the optimalreference velocities. With the use of this algorithm, Optimization 1 issolved with quadratic cost, C(u_(i:m)), or arbitrary utilitiesφ_(i)(u_(i)), linearized inter-agent collision avoidance constraints,non-convex motion constraints, and static obstacle avoidanceconstraints. Computational complexity is low and stability is good, butthe solution space is reduced due to the linearization of thecollision-avoidance constraints and the partition of the referencevelocity space to guarantee safety in the distributed case.

These algorithms provide either collision-free reference inputs thatrespect all the dynamic constraints (u*_(i) if distributed or u*_(1:m)if centralized) or an infeasible flag. If the optimization isinfeasible, the time horizon might be decreased or a differentlinearization might be selected. If the problem remains infeasible,agent i (distributed) or agents 1:m (centralized) decelerate on theirlast feasible local trajectory (obtained at time t_(f)) following thetime remap given by:

γ(t)=−t _(f) ²/(2τ)+(1+t _(f)/τ)t−t ²/(2τ), t ∈ [t _(f) ,t _(f)+τ],γ(t)=t _(f)+τ/2, t>t _(f)+τ  Eq. (68)

The re-parameterization of the trajectories guarantees that the vehiclesstop within the time horizon of the local planner, unless theoptimization becomes feasible in a later time step.

Turning first to distributed convex optimization, the optimization cost,C(u_(i)) is defined by a quadratic function given by two terms. Thefirst one is a regularizing term penalizing changes in velocity, and thesecond one minimizes the distance to the preferred velocity. Theoptimization cost may be determined as:

C(u _(i)):=K _(o) ∥u _(i) −v _(i)∥² +∥u _(i) −ū _(i)∥²   Eq. (69)

where K_(o) is a design constant.

Typically, pedestrians prefer to maintain a constant velocity in orderto minimize energy. Hence, preference was given to turning instead ofchanging speed. Furthermore, penalizing changes in speed leads to areduction of deadlock situations. This idea can be formalized as anelliptical cost, higher in the direction parallel to ū_(i) and lowerperpendicular to it. One can then let Λ<0 represent the relative weightand let γ_(i) represent the orientation of ū_(i). Then, the relativeweighting is defined by:

$L = {{\begin{bmatrix}\Lambda & 0 \\0 & 1\end{bmatrix}\mspace{14mu} {and}\mspace{14mu} D_{i}} = \begin{bmatrix}{\cos \; \gamma_{i}} & {\sin \; \gamma_{i}} \\{{- \sin}\; \gamma_{i}} & {\cos \; \gamma_{i}}\end{bmatrix}}$

And the optimization cost is redefined as:

C(u _(i)):=K _(o) ∥u _(i) −v _(i)∥²+(u _(i) −ū _(i))^(T) D _(i) ^(T) LD_(i)(u _(i) −ū _(i))   Eq. (70)

For each neighboring agent j, a collision-avoidance constraint iscomputed, linearized, and partitioned (u_(i) ∈ F_(i|j)).

The constraints for avoidance of static obstacles are also linearized,and the set, R_(i), defining the motion constraints is approximated by aconvex polygon (formed by a union of linear constraints). If R_(i) isgiven by two convex regions, only one is selected such as either theclosest one to the previous reference velocity or the closest one to thepreferred reference velocity. Note that at high speeds, R_(i) is formedby a single region. The set of all linear constraints may be denoted byC_(lin). Then, Optimization 1 may be formulated as a quadratic programwith linear constraints as:

$\begin{matrix}{{u_{i}^{*} = {\arg \begin{matrix}\min \\u_{i}\end{matrix}{C\left( u_{i} \right)}}}{{s.t.\mspace{14mu} {u_{i}}} \leq u_{\max}}{{n_{s} \cdot u_{i}} \leq {b_{s}{\forall{s \in C_{lin}}}}}} & {{Eq}.\mspace{11mu} (71)}\end{matrix}$

Turning now to centralized convex optimization, the optimization cost,C(u_(1:m)), is defined by a quadratic function given by a weighted sumof the cost functions, C(u_(i)), of each agent as:

C(u _(1:m)):=Σ_(i=1) ^(m)ω_(i) C(u _(i))   Eq. (72)

where ω_(i) represents the weight of each individual agent in theavoidance effort and can be used to define characteristics such as shyversus aggressive behavior.

For each neighboring pair of neighboring agents, i and j, acollision-avoidance constraint is computed and linearized (e.g., asdescribed above). The set of linear constraints for inter-agentcollision avoidance (one for each pair of neighboring agents) is denotedby C_(rel). The constraints for avoidance of static obstacles are alsolinearized and the sets R_(i) are approximated by a convex polygon. Theset of these linear constraints for agent i is denoted by C_(i,abs).Then, Optimization 2 can be formulated as a quadratic program withlinear constraints as:

$\begin{matrix}{{u_{1\text{:}m}^{*} = {\arg_{u_{1\text{:}m}}^{\min}{C\left( u_{i\text{:}m} \right)}}}{{{s.t.{u_{i}}} \leq u_{\max}},{\forall{i \leq m}}}{{{n_{ij} \cdot \left( {u_{i} - u_{j}} \right)} \leq b_{ij}},{\forall i},{j\mspace{14mu} {neighboring}\mspace{14mu} {agents}}}{{{n_{s} \cdot u_{i}} \leq b_{s}},{\forall{i \in \left\lbrack {1,m} \right\rbrack}},{\forall{s \in _{i,{abs}}}}}} & {{Eq}.\mspace{14mu} 73}\end{matrix}$

Turning now to centralized non-convex optimization, to coin the problemas a convex optimization, the inter-agent collision avoidanceconstraints were linearized, which led to a sub-optimal solution due tothe reduction of the solution space (an avoidance topology was fixed).To obtain the global optimum, a non-convex problem should be solved. Thesolution may be obtained via sampling, which may use a large amount ofsamples in this high dimensional space with many constraints. Instead,the inventors propose that one of the two following algorithms may beutilized to provide shared control of vehicles.

First, one may utilize a mixed integer quadratic program. Theoptimization that is formulated in this way uses a program where threebinary variables are added for each inter-agent collision avoidanceconstraint and that specifies which one of the three linear constraintsis active. Another two binary variables are added for each R_(i) setthat needs to be approximated by two convex regions. One can then let bbe the vector of all binary variables, where each binary variable, β, iszero if the constraint is active (to be satisfied) and one otherwise.

The optimization cost of Equation (72) is kept, with an optionalmodification. Since an avoidance topology is not defined beforehand, aside preference for the collision avoidance can now be added byappropriately selecting a penalty for the binary variables. The threebinary variables associated with each VO constraint, s ∈ C_(rel), aredenoted by β_(s) ^(l), l ∈ [1,3]. For example, the rule “prefer to avoidon the right” can be added by penalizing β_(s) ^(l)=1 with a weight w.The vector of linear cost with respect to the binary variables is thengiven by:

${f_{\beta} = {\underset{\underset{3{C_{rel}}}{}}{\left\lbrack {{w00}\mspace{14mu} \ldots \mspace{14mu} {w00}} \right.}\underset{\underset{2{I_{2}}}{}}{\left. {00\mspace{14mu} \ldots \mspace{14mu} 00} \right\rbrack}}},$

where I₂ is the set of agents whose set R_(i) is approximated by twoconvex regions, I₁=[1,m]\I₂, and |X| the cardinality of set X. The setof linear constraints for static obstacle avoidance is denoted byC_(i,obst) and the set of linear constraints defining the polygonalapproximation of R_(i) by C_(i,dyn), and the sets verifyC_(i,abs)=C_(i,obst) ∪ C_(i,dyn).

Optimization 2, in this case, can then be formulated as a quadraticprogram with linear constraints as:

$\begin{matrix}{{u_{1:m}^{*} = {{\arg \begin{matrix}\min \\{u_{1:m}b}\end{matrix}\left( {Cu}_{i:m} \right)} + {f_{\beta} \cdot b}}}{{{s.t.\mspace{11mu} {{u_{i}}}} \leq u_{\max}},{\forall{i \in \left\lbrack {1,m} \right\rbrack}}}{{{{n_{s}^{l} \cdot \left( {u_{i} - u_{j}} \right)} - {\beta_{s}^{l}M}} \leq b_{s}^{l}},{\forall{s \in _{rel}}},{l \in {\left\lbrack {1,3} \right\rbrack {{\beta_{s}^{l} + \beta_{s}^{2} + \beta_{s}^{3}} = 2}}},{\forall{s \in _{rel}}}}{{{n_{s} \cdot u_{i}} \leq b_{s}},{\forall{i \in \left\lbrack {1,m} \right\rbrack}},{\forall{s \in _{i,{obst}}}}}{{{n_{s} \cdot u_{i}} \leq b_{s}},{\forall{i \in I_{1}}},{\forall{s \in _{i,{dyn}}}}}{{{{n_{s} \cdot u_{i}} - {\beta_{s}^{l}M}} \leq b_{s}},{\forall{i \in I_{2}}},{\forall{s \in _{i,{dyn}}}},{l \in \left\lbrack {1,2} \right\rbrack}}{{{\beta_{s}^{1} + \beta_{s}^{2}} = 1},{\forall{i \in I_{2}}},{\forall{s \in _{i,{dyn}}}}}} & {{Eq}.\mspace{14mu} (74)}\end{matrix}$

where M>>0 is a large enough constant and extra constraints have beenadded to impose that only one linear constraint is active for eachoriginal non-convex constraint. This optimization can be solved viabranch-and bound, where the maximum number of explored nodes defines theratio between optimality and computational time.

To reduce the computational time of the algorithm, an initial point maybe specified. The solution of the convex optimization discussed hereinmay be well suited for this purpose since it typically returns afeasible (although not optimal) solution with very low computationaltime. Optimality of the solution increases with time as more nodes areexplored. The algorithm can exit at any time to cope with run-timeconstraints, and, hence, this method features anytime properties.

Second, centralized, non-convex optimization may be formulated asmessage passing optimization. For example, this non-convex optimizationcan be solved using an improved version of the alternating directionmethod of multipliers (ADMM) algorithm known to those skilled in theart. This approach was naturally parallelizable and allowed forincorporating different cost functional with only minor adjustments, andbinary constraints were no longer required and/or used. None the less,convergence to the global optimum was not guaranteed.

Turning now to distributed non-convex search within a convex region,convex (linear) and non-convex (grid-based) constraints are combined toexplore a larger solution space while keeping the computational costlow. A convex cost, C(u_(i)), is given by Eq. 70. Additionally, anon-convex cost can be defined, for example, by {tilde over(C)}(u_(i))=C(u_(i))+χ_(i)(u_(i)). The set of convex constraints isformed by the inter-agent linearized collision avoidance constraints(i.e., set F_(i|j)), and the bounding box of the set R_(i) is denoted byC. For agent i, the set of non-convex constraints is formed by thestatic obstacle collision avoidance constraint and the set R_(i)accounting for the motion constraints and is denoted by {tilde over(C)}_(i). Both non-convex constraints are given by grid representationsof identical resolution.

To efficiently find a solution, the optimization is divided in twoparts. First, a convex subproblem is solved resulting in u_(i) ^(c),and, second, this is followed by a search within the grid-basedconstraints restricted to the convex area defined by the linearconstraints. The quadratic cost defined in Equation (70) is maintained,and the algorithm proceeds as follows:

Input distributed: z₁, ū₁ and r₁ + ε_(i); p_(j), {dot over (p)}_(j) andr_(j) ∀j neighbor of 1. Consider ε_(j) = ε₁. Result: u_(i)*   Computeall constraints 1, 2 and 3   u_(i) ^(c) ← solution 2-dimensional convexoptimization with quadratic cost Eq. (70) and convex constraints C.  ||Wave expansion from u_(i) ^(c) within convex area C.   Initializesorted list L (increasing cost {tilde over (C)} (u_(i))) with u_(i)^(c).   while L ≠ Ø do     u_(i) ← first point in L; L := L\u_(i);feasible := ‘true’;     if feasibleDynamics(u_(i))= ‘false’ orfeasibleMap(u_(i))= ‘false’     then      L ←expandNeighbors(L, u_(i));     feasible := ‘false’;     end if     if feasible = ‘true’ thenreturn u_(i)* = u_(i) end if.   end while;   return 0;

In this algorithm, function feasibleDynamics(u_(i)), checks in aprecomputed grid if the tracking error is below ε_(i), given the initialstate of the vehicle (if u_(i) ∈ R_(i) then return ‘true’; else‘false’). Function feasibleMap(u _(i)) checks if u_(i) leads to atrajectory in collision with static obstacles given by the grid mapO_(r) _(i) _(+ε) _(i) . This is efficiently checked in the precomputeddilated map (see Constraint 2) (if segment (p_(i),p_(i)+u_(i)τ) ∩ O_(r)_(i) _(+ε) _(i) =Ø then return ‘true’). The function expandNeighborsadds the neighboring grid points if they are within the convex regiondefined by the convex constraints in and they are not previouslyexplored, with the function defined as:

Function L ← expandNeighbors(L, u_(i) ^(in)); for each 8-connected gridneighbor u_(i) ^(neighbor) of u_(i) ^(in) do   if {u_(i) ^(neighbor) notpreviously added to L} and   {u_(i) ^(neighbor) satisfies convexconstraints C} then   L ← L ∪ u_(i) ^(neighbor);   Sort list L,increasing cost {tilde over (C)}(u_(i))   end if; end for;An example of implementation of the distributed non-convex search withina convex region algorithm is shown in FIG. 28 with data flow orfunctional block diagram 2800.

Now, it may be useful to turn to an analysis of the above-describedalgorithms providing collision avoidance guarantees. With regard toradius enlargement, variable maximum tracking error, ε_(i), andassociated radius enlargement is desirable for feasibility. To guaranteefeasibility of the computation of the inter-agent collision avoidanceconstraint, VO_(i|j) ^(τ), r_(i)+r_(j)+ε_(i)+ε_(j)≦d(p_(i),p_(j)) shouldbe satisfied, i.e., the extended radii of the robots should not be incollision. This might happen for fixed ε_(i) but is assuredly avoided byhaving ε_(i) and ε_(j) stepwise decreasing when robots are close to eachother.

The planned local trajectories are collision free up to time τ, with theassumption that other vehicles follow the same algorithm or maintain aconstant velocity. Passive safety is guaranteed if the assumptions hold.In the case where optimization is feasible, collision-free motion isguaranteed for the local trajectory up to time τ (optimal referencetrajectory is collision free for agents of radii enlarged by ε andagents stay within ε of it) with the assumption that all interactingagents either continue with their previous velocity or compute a new onefollowing the same algorithm and assumptions. One can let p_(i)(t)denote the position of agent i at time t≧t^(k) and recall {tilde over(t)}=t−t^(k), then:

∥p _(i)(t)−p _(j)(t)∥=∥f(z _(i) ^(k) ,u _(i) {tilde over (t)})−f(z _(j)^(k) ,u _(j) ,{tilde over (t)})∥≧∥(p _(i) ^(k) +u _(i) {tilde over(t)})−(p _(j) ^(k) +u _(j) {tilde over (t)})∥−ε_(i)−ε_(j) ≧r _(i)+ε_(i)+r _(j)+ε_(j)−ε_(i)−ε_(j) =r _(i) +r _(j)   Eq. (75)

where the first inequality holds from the triangular inequality andConstraint 1 (u_(i) ∈ R_(i) and u_(j) ∈ R_(j)). The second inequalityholds from Constraint 3 (u_(i)−u_(j) ∉ VOT_(ij) ^(τ) and, if distributedu_(i) ∈ F_(i|j) and u_(j) ∈ F_(j|i)). To guarantee feasibility of thevelocity obstacles computation and, thus, completion of the method, thediscussion regarding radius enlargement should also hold. Due to thetime-discrete implementation, after each time step, a new collision-freetrajectory is computed. Therefore, the trajectories of all agents, givenas concatenation of segments, are collision free.

If the optimization is infeasible, no collision-free solution existsthat respects all constraints. If the time horizon is longer than therequired time to stop, safety is preserved if all involved vehiclesdrive their last feasible trajectory with a time re-parameterization toreach stop before a collision arises:

{dot over (γ)}(t _(f)+τ)=(1+t _(f)/τ)−(t _(f)+τ)/τ=0   Eq. (76)

This time re-parameterization implies a slowdown of the agent, which inturn may render the optimization feasible in a later time step. Sincethis computation is performed at a high frequency, each individual agentis able to adapt to changing situations. With regard to dynamicobstacles, the feasibility of the optimization indicates if the dynamicobstacles can be avoided (assuming they adhere to their predictedreference velocity) or a collision is imminent. A fast control loop isable to handle small deviations in the prediction.

Although the invention has been described and illustrated with a certaindegree of particularity, it is understood that the present disclosurehas been made only by way of example, and that numerous changes in thecombination and arrangement of parts can be resorted to by those skilledin the art without departing from the spirit and scope of the invention,as hereinafter claimed. For example, in addition to the test casesdescribed herein, a test case may involve arbitrary robots moving amongpeople such as in a theme park or on a busy street (e.g., either withpassenger providing input (such as a Segway-type vehicle or awheelchair) or with autonomous control (e.g., a robotic character, acleaning robot, a surveillance device, and so on)).

It will be understood that the methods taught herein are also useful forautonomous driving and not only semi-autonomous. For example, if noinput from a driver/guest is available, the vehicle can be controlled tofollow a desired path or navigate toward a predefined goal by itself.Thus, the driver input is optional. One can imagine a case where adriver/guest sits in/on the vehicle but does not command anything so asto just go along for a ride provided by the autonomously controlledvehicle. This one more reason that the guidance trajectories can bebeneficial.

Several means are available to implement the systems and methodsdiscussed in this specification. These means include, but are notlimited to, digital computer systems, microprocessors,application-specific integrated circuits (ASIC), general purposecomputers, programmable controllers and field programmable gate arrays(FPGAs), all of which may be generically referred to herein as“processors” (such as represented by processors 112 and 122 in thesystem 100 of FIG. 1). For example, in one embodiment, signal processingmay be incorporated by an FPGA or an ASIC, or alternatively by anembedded or discrete processor. Therefore, other embodiments includeprogram instructions resident on computer readable media which whenimplemented by such means enable them to implement various embodiments.Computer readable media include any form of a non-transient physicalcomputer memory device (such as memory 149 of control system 120 in FIG.1). Examples of such a physical computer memory device include, but arenot limited to, punch cards, magnetic disks or tapes, optical datastorage systems, flash read only memory (ROM), non-volatile ROM,programmable ROM (PROM), erasable-programmable ROM (E-PROM), randomaccess memory (RAM), or any other form of permanent, semi-permanent, ortemporary memory storage system or device. Program instructions (such asthose in controllers 116, 132 in FIG. 1) include, but are not limitedto, computer-executable instructions executed by computer systemprocessors and hardware description languages such as Very High SpeedIntegrated Circuit (VHSIC) Hardware Description Language (VHDL).

We claim:
 1. A method of providing shared control over movement of avehicle within a space, comprising: receiving user input related to avelocity for the vehicle within the space; processing the user inputwith a local motion planner selectively adjusting the velocity based ona set of predefined constraints and presence of neighboring vehicles inthe space to generate a trajectory for the vehicle in the space;communicating motion control signals including the trajectory to thevehicle; and operating drive mechanisms in the vehicle based on themotion control signals to move the vehicle from a first position to asecond position within the space for an upcoming time period.
 2. Themethod of claim 1, wherein the trajectory defines a speed for thevehicle and a direction for movement.
 3. The method of claim 1, whereinthe set of predefined constraints comprises a grid map of the space thatdefines locations of obstacles in the space and wherein the processingof the user input includes searching the grid map to identify theobstacles and to define a path from the first position to the secondposition in the space that avoids the obstacles.
 4. The method of claim3, wherein the obstacles comprise virtual objects displayed in the spaceor on a monitor in the vehicle.
 5. The method of claim 1, wherein theset of predefined constraints comprises a guidance trajectory defining apassageway through the space that can be traveled by the vehicle withoutcollision with the obstacles and wherein the trajectory is defined toretain the vehicle within the passageway.
 6. The method of claim 5,wherein the passageway comprises a set of interconnected waypoints inthe space and wherein an active area in which the vehicle may move isassociated with each of the waypoints.
 7. The method of claim 6, whereinat least some of the active areas differ in size or shape definingdiffering areas in which the vehicle may move within the space.
 8. Themethod of claim 1, wherein the set of predefined constraints includeskinematic data for the vehicle and wherein the processing of the userinput based on presence of neighboring vehicles includes processing datafor collision avoidance for other vehicles moving in the space, velocitypredictions for ones of the other vehicles neighboring the vehicle inthe space, or predictions of movements of the other vehicles during theupcoming time period.
 9. A method for controlling vehicle movementthrough a drive or air space, comprising: with a controller, receivinguser input related to a user-preferred trajectory for moving the vehiclewithin the space; and with the controller, processing the user input toselectively adjust the trajectory based on a set of predefinedconstraints to generate a control trajectory for the vehicle in thespace for an upcoming time period, wherein the set of predefinedconstraints comprises a grid map of the space that defines locations ofobstacles in the space and wherein the processing of the user inputincludes searching the grid map to identify the obstacles and to definea path from the first position to the second position in the space thatavoids the obstacles.
 10. The method of claim 9, wherein the set ofpredefined constraints comprises a guidance trajectory defining apassageway through the space that can be traveled by the vehicle withoutcollision with the obstacles and wherein the trajectory is defined toretain the vehicle within the passageway.
 11. The method of claim 10,wherein the passageway comprises a set of interconnected waypoints inthe space, wherein an active area in which the vehicle may move isassociated with each of the waypoints, and wherein at least some of theactive areas differ in size or shape defining differing areas in whichthe vehicle may move within the space.
 12. The method of claim 9,wherein the set of predefined constraints includes kinematic data forthe vehicle, data for collision avoidance for other vehicles moving inthe space, velocity predictions for ones of the other vehiclesneighboring the vehicle in the space, or predictions of movements of theother vehicles during the upcoming time period.
 13. A method forcontrolling a vehicle in an installation, comprising: receiving auser-generated vehicle motion command for the vehicle; determiningwhether the user-generated vehicle motion command implicates aconstraint; when the user-generated vehicle motion command implicatesthe constraint, combining the user-generated vehicle motion command witha system-generated command to produce a modified vehicle motion commandto control movement of the vehicle in the installation; and when theuser-generated vehicle motion command does not implicate the constraint,using the user-generated vehicle motion command to control movement ofthe vehicle in the installation.
 14. The method of claim 13, wherein theconstraint comprises a schedule such that the vehicle is guaranteed tobe in a specified zone in the installation at a system-determined timeor time interval.
 15. The method of claim 13, wherein the schedulefurther defines an orientation for the vehicle within the specifiedzone.
 16. The method of claim 13, wherein the constraint comprises arelationship between the vehicle and at least one other vehicle movingthrough the installation and wherein the user-generated vehicle motioncommand is modified according to the relationship.
 17. The method ofclaim 13, wherein the constraint comprises a vehicle position relativeto static obstacles in the installation and wherein the user-generatedvehicle motion command is modified according to a position of thevehicle in the installation relative to the static obstacles.
 18. Themethod of claim 13, further comprising projecting an image of an objectonto a surface in the installation and wherein the modified vehiclemotion command is generated based on a proximity of the vehicle to thesurface onto which the image is projected.
 19. The method of claim 18,wherein the modified vehicle motion command is adjusted when the vehiclepasses over the surface onto which the image is projected or wherein theimage is a fixed or moving projected image generated in response to userinput in the vehicle.
 20. The method of claim 13, wherein the combiningof the user-generated vehicle motion command with the system-generatedcommand to produce the modified vehicle motion command comprisesperforming an optimization adapted to maintain a perception that theuser-generated vehicle motion command is directly controlling thevehicle subject to the constraint and wherein the constraint comprisesat least one of avoiding collision with other vehicles moving in theinstallation, avoiding collision with static obstacles in theinstallation, and ensuring a defined motion is realizable by thevehicle.
 21. The method of claim 20, wherein the optimization is globalwhereby motion commands are computed concurrently for all vehicles,including the vehicle, in the installation.
 22. The method of claim 20,wherein the optimization is local to each vehicle in the installationbased on the user-generated vehicle motion command and a predictedmotion of neighboring vehicles of the vehicle.
 23. The method of claim20, wherein, when the modified vehicle motion command does not satisfythe constraint, a responsive motion command is generated to stop thevehicle or to continue movement of the vehicle along a prior trajectory.24. The method of claim 20, wherein surfaces of the installation arecharacterized by a pixel map comprising a two-dimensional orthree-dimensional grid allowing computation of vehicle motions that donot result in collision with the surfaces.
 25. The method of claim 20,wherein the constraint is precomputed with respect to a vehiclecontroller.
 26. The method of claim 20, wherein a solution of theoptimization is determined by wave expansion within a convex areadefined by half-plane constraints determined to avoid collision withother vehicles until a value is determined that satisfies the constraintto avoid collision with fixed surface of the installation and satisfiesthe constraint to ensure the defined motion is realizable.
 27. Themethod of claim 14, wherein the schedule is defined using a guidancetrajectory specifying a trajectory, a time associated with points on thetrajectory, an active zone associated with each of the points, and atime interval associated with each of the active zones and wherein 28.The method of claim 27, wherein the constraint comprises ensuring aposition of the vehicle lies within one of the active zones within thetime interval associated with the one of the active zones.
 29. Themethod of claim 28, wherein the schedule is defined by a second guidancetrajectory, wherein the vehicle may be controlled in a selective mannerusing the guidance trajectory or the second guidance trajectory, andwherein switching between the guidance trajectories is performed inresponse to optimizing a cost function related to a vehicle schedulebased on a current state of the vehicle or is performed in response touser input selecting among a two or more of the active states.
 30. Themethod of 28, wherein consistency between the vehicle and the guidancetrajectory is maintained by adding a motion component that directs thevehicle towards a point within the guidance trajectory for a next timeinstant.